Skip to content

Commit

Permalink
Added const to moveit_core/collision_detection per issue 879 (#1416)
Browse files Browse the repository at this point in the history
  • Loading branch information
@bgill92
bgill92 committed Jul 12, 2022
1 parent 18ffec0 commit 2de48a7
Show file tree
Hide file tree
Showing 6 changed files with 85 additions and 83 deletions.
34 changes: 17 additions & 17 deletions moveit_core/collision_detection/src/collision_env.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
// Logger
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_robot");

static inline bool validateScale(double scale)
static inline bool validateScale(const double scale)
{
if (scale < std::numeric_limits<double>::epsilon())
{
Expand All @@ -57,7 +57,7 @@ static inline bool validateScale(double scale)
return true;
}

static inline bool validatePadding(double padding)
static inline bool validatePadding(const double padding)
{
if (padding < 0.0)
{
Expand All @@ -83,7 +83,7 @@ CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double
padding = 0.0;

const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
for (auto link : links)
for (const auto link : links)
{
link_padding_[link->getName()] = padding;
link_scale_[link->getName()] = scale;
Expand All @@ -100,7 +100,7 @@ CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, const
padding = 0.0;

const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
for (auto link : links)
for (const auto link : links)
{
link_padding_[link->getName()] = padding;
link_scale_[link->getName()] = scale;
Expand All @@ -113,13 +113,13 @@ CollisionEnv::CollisionEnv(const CollisionEnv& other, const WorldPtr& world)
link_padding_ = other.link_padding_;
link_scale_ = other.link_scale_;
}
void CollisionEnv::setPadding(double padding)
void CollisionEnv::setPadding(const double padding)
{
if (!validatePadding(padding))
return;
std::vector<std::string> u;
const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
for (auto link : links)
for (const auto link : links)
{
if (getLinkPadding(link->getName()) != padding)
u.push_back(link->getName());
Expand All @@ -129,13 +129,13 @@ void CollisionEnv::setPadding(double padding)
updatedPaddingOrScaling(u);
}

void CollisionEnv::setScale(double scale)
void CollisionEnv::setScale(const double scale)
{
if (!validateScale(scale))
return;
std::vector<std::string> u;
const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
for (auto link : links)
for (const auto link : links)
{
if (getLinkScale(link->getName()) != scale)
u.push_back(link->getName());
Expand All @@ -145,10 +145,10 @@ void CollisionEnv::setScale(double scale)
updatedPaddingOrScaling(u);
}

void CollisionEnv::setLinkPadding(const std::string& link_name, double padding)
void CollisionEnv::setLinkPadding(const std::string& link_name, const double padding)
{
validatePadding(padding);
bool update = getLinkPadding(link_name) != padding;
const bool update = getLinkPadding(link_name) != padding;
link_padding_[link_name] = padding;
if (update)
{
Expand All @@ -172,7 +172,7 @@ void CollisionEnv::setLinkPadding(const std::map<std::string, double>& padding)
for (const auto& link_pad_pair : padding)
{
validatePadding(link_pad_pair.second);
bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
const bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
link_padding_[link_pad_pair.first] = link_pad_pair.second;
if (update)
u.push_back(link_pad_pair.first);
Expand All @@ -186,10 +186,10 @@ const std::map<std::string, double>& CollisionEnv::getLinkPadding() const
return link_padding_;
}

void CollisionEnv::setLinkScale(const std::string& link_name, double scale)
void CollisionEnv::setLinkScale(const std::string& link_name, const double scale)
{
validateScale(scale);
bool update = getLinkScale(link_name) != scale;
const bool update = getLinkScale(link_name) != scale;
link_scale_[link_name] = scale;
if (update)
{
Expand All @@ -200,7 +200,7 @@ void CollisionEnv::setLinkScale(const std::string& link_name, double scale)

double CollisionEnv::getLinkScale(const std::string& link_name) const
{
auto it = link_scale_.find(link_name);
const auto it = link_scale_.find(link_name);
if (it != link_scale_.end())
return it->second;
else
Expand All @@ -212,7 +212,7 @@ void CollisionEnv::setLinkScale(const std::map<std::string, double>& scale)
std::vector<std::string> u;
for (const auto& link_scale_pair : scale)
{
bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
const bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
link_scale_[link_scale_pair.first] = link_scale_pair.second;
if (update)
u.push_back(link_scale_pair.first);
Expand All @@ -232,7 +232,7 @@ void CollisionEnv::setPadding(const std::vector<moveit_msgs::msg::LinkPadding>&
for (const auto& p : padding)
{
validatePadding(p.padding);
bool update = getLinkPadding(p.link_name) != p.padding;
const bool update = getLinkPadding(p.link_name) != p.padding;
link_padding_[p.link_name] = p.padding;
if (update)
u.push_back(p.link_name);
Expand All @@ -247,7 +247,7 @@ void CollisionEnv::setScale(const std::vector<moveit_msgs::msg::LinkScale>& scal
for (const auto& s : scale)
{
validateScale(s.scale);
bool update = getLinkScale(s.link_name) != s.scale;
const bool update = getLinkScale(s.link_name) != s.scale;
link_scale_[s.link_name] = s.scale;
if (update)
u.push_back(s.link_name);
Expand Down
43 changes: 22 additions & 21 deletions moveit_core/collision_detection/src/collision_matrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ AllowedCollisionMatrix::AllowedCollisionMatrix()
{
}

AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed)
AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, const bool allowed)
{
for (std::size_t i = 0; i < names.size(); ++i)
for (std::size_t j = i; j < names.size(); ++j)
Expand Down Expand Up @@ -105,10 +105,10 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo

bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const
{
auto it1 = allowed_contacts_.find(name1);
const auto it1 = allowed_contacts_.find(name1);
if (it1 == allowed_contacts_.end())
return false;
auto it2 = it1->second.find(name2);
const auto it2 = it1->second.find(name2);
if (it2 == it1->second.end())
return false;
fn = it2->second;
Expand All @@ -118,7 +118,7 @@ bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::strin
bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2,
AllowedCollision::Type& allowed_collision) const
{
auto it1 = entries_.find(name1);
const auto it1 = entries_.find(name1);
if (it1 == entries_.end())
return false;
auto it2 = it1->second.find(name2);
Expand All @@ -135,14 +135,14 @@ bool AllowedCollisionMatrix::hasEntry(const std::string& name) const

bool AllowedCollisionMatrix::hasEntry(const std::string& name1, const std::string& name2) const
{
auto it1 = entries_.find(name1);
const auto it1 = entries_.find(name1);
if (it1 == entries_.end())
return false;
auto it2 = it1->second.find(name2);
const auto it2 = it1->second.find(name2);
return it2 != it1->second.end();
}

void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, bool allowed)
void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, const bool allowed)
{
const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
entries_[name1][name2] = entries_[name2][name1] = v;
Expand Down Expand Up @@ -213,21 +213,22 @@ void AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::st
}
}

void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed)
void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names,
const bool allowed)
{
for (const auto& other_name : other_names)
if (other_name != name)
setEntry(other_name, name, allowed);
}

void AllowedCollisionMatrix::setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2,
bool allowed)
const bool allowed)
{
for (const auto& name1 : names1)
setEntry(name1, names2, allowed);
}

void AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
void AllowedCollisionMatrix::setEntry(const std::string& name, const bool allowed)
{
std::string last = name;
for (auto& entry : entries_)
Expand All @@ -238,15 +239,15 @@ void AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
}
}

void AllowedCollisionMatrix::setEntry(bool allowed)
void AllowedCollisionMatrix::setEntry(const bool allowed)
{
const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
for (auto& entry : entries_)
for (auto& it2 : entry.second)
it2.second = v;
}

void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, bool allowed)
void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, const bool allowed)
{
const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
default_entries_[name] = v;
Expand Down Expand Up @@ -285,12 +286,12 @@ static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f
bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
DecideContactFn& fn) const
{
bool found = getEntry(name1, name2, fn);
const bool found = getEntry(name1, name2, fn);
if (!found)
{
DecideContactFn fn1, fn2;
bool found1 = getDefaultEntry(name1, fn1);
bool found2 = getDefaultEntry(name2, fn2);
const bool found1 = getDefaultEntry(name1, fn1);
const bool found2 = getDefaultEntry(name2, fn2);
if (found1 && !found2)
fn = fn1;
else if (!found1 && found2)
Expand All @@ -307,8 +308,8 @@ bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name1, const std
AllowedCollision::Type& allowed_collision) const
{
AllowedCollision::Type t1, t2;
bool found1 = getDefaultEntry(name1, t1);
bool found2 = getDefaultEntry(name2, t2);
const bool found1 = getDefaultEntry(name1, t1);
const bool found2 = getDefaultEntry(name2, t2);
if (!found1 && !found2)
return false;
else if (found1 && !found2)
Expand Down Expand Up @@ -373,7 +374,7 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix
for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
{
AllowedCollision::Type dtype;
bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
const bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
if (dfound)
{
msg.default_entry_names.push_back(msg.entry_names[i]);
Expand All @@ -395,9 +396,9 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
getAllEntryNames(names);

std::size_t spacing = 4;
for (auto& name : names)
for (const auto& name : names)
{
std::size_t length = name.length();
const std::size_t length = name.length();
if (length > spacing)
spacing = length;
}
Expand Down Expand Up @@ -436,7 +437,7 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
// print pairs
for (std::size_t j = 0; j < names.size(); ++j)
{
bool found = getAllowedCollision(names[i], names[j], type);
const bool found = getAllowedCollision(names[i], names[j], type);
if (found)
out << std::setw(3) << indicator[type];
else
Expand Down
Loading

0 comments on commit 2de48a7

Please sign in to comment.