Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added brace intialization in moveit_core/collision_detection_fcl & moveit_core/collision_detection_field #1622

Merged
merged 2 commits into from
Oct 18, 2022
Merged

Added brace intialization in moveit_core/collision_detection_fcl & moveit_core/collision_detection_field #1622

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9e776fb
added brace intialization in moveit_core/collision_detection_fcl
abhijelly Oct 18, 2022
00296f5
added brace initialization in moveit_core/collision_detection_fcl & m…
abhijelly Oct 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions moveit_core/collision_detection_fcl/src/collision_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
RCLCPP_DEBUG(LOGGER, "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());

// see if we need to compute a contact
std::size_t want_contact_count = 0;
std::size_t want_contact_count{ 0 };
if (cdata->req_->contacts)
if (cdata->res_->contact_count < cdata->req_->max_contacts)
{
Expand Down Expand Up @@ -249,7 +249,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
// otherwise, we need to compute more things
bool enable_cost = cdata->req_->cost;
std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
bool enable_contact = true;
bool enable_contact{ true };

fcl::CollisionResultd col_result;
int num_contacts =
Expand Down
12 changes: 6 additions & 6 deletions moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
// we keep the same order of objects as what RobotState *::getLinkState() returns
for (auto link : links)
for (std::size_t j = 0; j < link->getShapes().size(); ++j)
for (std::size_t j{ 0 }; j < link->getShapes().size(); ++j)
{
FCLGeometryConstPtr link_geometry = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
getLinkPadding(link->getName()), link, j);
Expand Down Expand Up @@ -122,7 +122,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
// we keep the same order of objects as what RobotState *::getLinkState() returns
for (auto link : links)
for (std::size_t j = 0; j < link->getShapes().size(); ++j)
for (std::size_t j{ 0 }; j < link->getShapes().size(); ++j)
{
FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
getLinkPadding(link->getName()), link, j);
Expand Down Expand Up @@ -175,7 +175,7 @@ void CollisionEnvFCL::getAttachedBodyObjects(const moveit::core::AttachedBody* a
std::vector<FCLGeometryConstPtr>& geoms) const
{
const std::vector<shapes::ShapeConstPtr>& shapes = ab->getShapes();
const size_t num_shapes = shapes.size();
const size_t num_shapes{ shapes.size() };
geoms.reserve(num_shapes);
for (std::size_t i = 0; i < num_shapes; ++i)
{
Expand All @@ -188,7 +188,7 @@ void CollisionEnvFCL::getAttachedBodyObjects(const moveit::core::AttachedBody* a

void CollisionEnvFCL::constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const
{
for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
for (std::size_t i{ 0 }; i < obj->shapes_.size(); ++i)
{
FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
if (g)
Expand All @@ -205,7 +205,7 @@ void CollisionEnvFCL::constructFCLObjectRobot(const moveit::core::RobotState& st
fcl_obj.collision_objects_.reserve(robot_geoms_.size());
fcl::Transform3d fcl_tf;

for (std::size_t i = 0; i < robot_geoms_.size(); ++i)
for (std::size_t i{ 0 }; i < robot_geoms_.size(); ++i)
if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_)
{
transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link,
Expand Down Expand Up @@ -445,7 +445,7 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector<std::string>& li
const moveit::core::LinkModel* lmodel = robot_model_->getLinkModel(link);
if (lmodel)
{
for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j)
for (std::size_t j{ 0 }; j < lmodel->getShapes().size(); ++j)
{
FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()),
getLinkPadding(lmodel->getName()), lmodel, j);
Expand Down
14 changes: 7 additions & 7 deletions moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ struct BodyDecompositionCache
BodyDecompositionCache() : clean_count_(0)
{
}
static const unsigned int MAX_CLEAN_COUNT = 100;
static const unsigned int MAX_CLEAN_COUNT{ 100 };
Map map_;
unsigned int clean_count_;
std::mutex lock_;
Expand Down Expand Up @@ -109,7 +109,7 @@ PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const m
double resolution)
{
PosedBodySphereDecompositionVectorPtr ret = std::make_shared<PosedBodySphereDecompositionVector>();
for (unsigned int i = 0; i < att->getShapes().size(); ++i)
for (unsigned int i{ 0 }; i < att->getShapes().size(); ++i)
{
PosedBodySphereDecompositionPtr pbd(
new PosedBodySphereDecomposition(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution)));
Expand All @@ -123,7 +123,7 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const mov
double resolution)
{
PosedBodyPointDecompositionVectorPtr ret = std::make_shared<PosedBodyPointDecompositionVector>();
for (unsigned int i = 0; i < att->getShapes().size(); ++i)
for (unsigned int i{ 0 }; i < att->getShapes().size(); ++i)
{
PosedBodyPointDecompositionPtr pbd =
std::make_shared<PosedBodyPointDecomposition>(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution));
Expand Down Expand Up @@ -169,8 +169,8 @@ void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& g
sphere_marker.lifetime = rclcpp::Duration(0, 0);

const moveit::core::RobotState& state = *(gsr->dfce_->state_);
unsigned int id = 0;
for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); ++i)
unsigned int id{ 0 };
for (unsigned int i{ 0 }; i < gsr->dfce_->link_names_.size(); ++i)
{
const moveit::core::LinkModel* ls = state.getLinkModel(gsr->dfce_->link_names_[i]);
if (gsr->dfce_->link_has_geometry_[i])
Expand All @@ -194,7 +194,7 @@ void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& g

sphere_marker.ns = attached_ns;
sphere_marker.color = attached_color;
for (unsigned int i = 0; i < gsr->dfce_->attached_body_names_.size(); ++i)
for (unsigned int i{ 0 }; i < gsr->dfce_->attached_body_names_.size(); ++i)
{
const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
if (!att)
Expand All @@ -212,7 +212,7 @@ void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& g
continue;
}

for (unsigned int j = 0; j < att->getShapes().size(); ++j)
for (unsigned int j{ 0 }; j < att->getShapes().size(); ++j)
{
PosedBodySphereDecompositionVectorPtr sphere_decp = gsr->attached_body_decompositions_[i];
sphere_decp->updatePose(j, att->getGlobalCollisionBodyTransforms()[j]);
Expand Down
56 changes: 28 additions & 28 deletions moveit_core/collision_distance_field/src/collision_distance_field_types.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@
#include <rclcpp/time.hpp>
#include <memory>

const static double EPSILON = 0.0001;
const static double EPSILON{ 0.0001 };

namespace collision_detection
{
Expand All @@ -64,10 +64,10 @@ std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body,
bodies::BoundingCylinder cyl;
body->computeBoundingCylinder(cyl);
unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0));
double spacing = cyl.length / ((num_points * 1.0) - 1.0);
double spacing{ cyl.length / ((num_points * 1.0) - 1.0) };
relative_transform = cyl.pose;

for (unsigned int i = 1; i < num_points - 1; i++)
for (unsigned int i{ 1 }; i < num_points - 1; i++)
{
collision_detection::CollisionSphere cs(
relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius);
Expand All @@ -86,8 +86,8 @@ bool PosedDistanceField::getCollisionSphereGradients(const std::vector<Collision
{
// assumes gradient is properly initialized

bool in_collision = false;
for (unsigned int i = 0; i < sphere_list.size(); ++i)
bool in_collision{ false };
for (unsigned int i{ 0 }; i < sphere_list.size(); ++i)
{
Eigen::Vector3d p = sphere_centers[i];
Eigen::Vector3d grad(0, 0, 0);
Expand Down Expand Up @@ -149,8 +149,8 @@ bool getCollisionSphereGradients(const distance_field::DistanceField* distance_f
{
// assumes gradient is properly initialized

bool in_collision = false;
for (unsigned int i = 0; i < sphere_list.size(); ++i)
bool in_collision{ false };
for (unsigned int i{ 0 }; i < sphere_list.size(); ++i)
{
Eigen::Vector3d p = sphere_centers[i];
Eigen::Vector3d grad;
Expand Down Expand Up @@ -207,7 +207,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f
const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value,
double tolerance)
{
for (unsigned int i = 0; i < sphere_list.size(); ++i)
for (unsigned int i{ 0 }; i < sphere_list.size(); ++i)
{
Eigen::Vector3d p = sphere_centers[i];
Eigen::Vector3d grad;
Expand Down Expand Up @@ -239,7 +239,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f
{
Eigen::Vector3d p = sphere_centers[i];
Eigen::Vector3d grad;
bool in_bounds = true;
bool in_bounds{ true };
double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
if (!in_bounds && (grad.norm() > 0))
{
Expand Down Expand Up @@ -287,7 +287,7 @@ void BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes, c
double resolution, double padding)
{
bodies_.clear();
for (unsigned int i = 0; i < shapes.size(); ++i)
for (unsigned int i{ 0 }; i < shapes.size(); ++i)
{
bodies_.addBody(shapes[i].get(), poses[i], padding);
}
Expand All @@ -297,7 +297,7 @@ void BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes, c
relative_collision_points_.clear();
std::vector<CollisionSphere> body_spheres;
EigenSTL::vector_Vector3d body_collision_points;
for (unsigned int i = 0; i < bodies_.getCount(); ++i)
for (unsigned int i{ 0 }; i < bodies_.getCount(); ++i)
{
body_spheres.clear();
body_collision_points.clear();
Expand All @@ -311,14 +311,14 @@ void BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes, c
}

sphere_radii_.resize(collision_spheres_.size());
for (unsigned int i = 0; i < collision_spheres_.size(); ++i)
for (unsigned int i{ 0 }; i < collision_spheres_.size(); ++i)
{
sphere_radii_[i] = collision_spheres_[i].radius_;
}

// computing bounding sphere
std::vector<bodies::BoundingSphere> bounding_spheres(bodies_.getCount());
for (unsigned int i = 0; i < bodies_.getCount(); ++i)
for (unsigned int i{ 0 }; i < bodies_.getCount(); ++i)
{
bodies_.getBody(i)->computeBoundingSphere(bounding_spheres[i]);
}
Expand Down Expand Up @@ -364,7 +364,7 @@ void PosedBodyPointDecomposition::updatePose(const Eigen::Isometry3d& trans)
{
posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());

for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); ++i)
for (unsigned int i{ 0 }; i < body_decomposition_->getCollisionPoints().size(); ++i)
{
posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
}
Expand All @@ -383,7 +383,7 @@ void PosedBodySphereDecomposition::updatePose(const Eigen::Isometry3d& trans)
{
// updating sphere centers
posed_bounding_sphere_center_ = trans * body_decomposition_->getRelativeBoundingSphere().center;
for (unsigned int i = 0; i < body_decomposition_->getCollisionSpheres().size(); ++i)
for (unsigned int i{ 0 }; i < body_decomposition_->getCollisionSpheres().size(); ++i)
{
sphere_centers_[i] = trans * body_decomposition_->getCollisionSpheres()[i].relative_vec_;
}
Expand All @@ -392,7 +392,7 @@ void PosedBodySphereDecomposition::updatePose(const Eigen::Isometry3d& trans)
if (!body_decomposition_->getCollisionPoints().empty())
{
posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); ++i)
for (unsigned int i{ 0 }; i < body_decomposition_->getCollisionPoints().size(); ++i)
{
posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
}
Expand All @@ -404,10 +404,10 @@ bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1,
{
Eigen::Vector3d p1_sphere_center = p1->getBoundingSphereCenter();
Eigen::Vector3d p2_sphere_center = p2->getBoundingSphereCenter();
double p1_radius = p1->getBoundingSphereRadius();
double p2_radius = p2->getBoundingSphereRadius();
double p1_radius{ p1->getBoundingSphereRadius() };
double p2_radius{ p2->getBoundingSphereRadius() };

double dist = (p1_sphere_center - p2_sphere_center).squaredNorm();
double dist{ (p1_sphere_center - p2_sphere_center).squaredNorm() };
return dist < (p1_radius + p2_radius);
}

Expand All @@ -416,13 +416,13 @@ void getCollisionSphereMarkers(const std_msgs::msg::ColorRGBA& color, const std:
const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
visualization_msgs::msg::MarkerArray& arr)
{
unsigned int count = 0;
unsigned int count{ 0 };
rclcpp::Clock ros_clock;
for (const auto& posed_decomposition : posed_decompositions)
{
if (posed_decomposition)
{
for (unsigned int j = 0; j < posed_decomposition->getCollisionSpheres().size(); ++j)
for (unsigned int j{ 0 }; j < posed_decomposition->getCollisionSpheres().size(); ++j)
{
visualization_msgs::msg::Marker sphere;
sphere.type = visualization_msgs::msg::Marker::SPHERE;
Expand Down Expand Up @@ -454,9 +454,9 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string&
(unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
return;
}
for (unsigned int i = 0; i < gradients.size(); ++i)
for (unsigned int i{ 0 }; i < gradients.size(); ++i)
{
for (unsigned int j = 0; j < gradients[i].distances.size(); ++j)
for (unsigned int j{ 0 }; j < gradients[i].distances.size(); ++j)
{
visualization_msgs::msg::Marker arrow_mark;
arrow_mark.header.frame_id = frame_id;
Expand All @@ -470,9 +470,9 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string&
arrow_mark.ns = ns;
}
arrow_mark.id = i * 1000 + j;
double xscale = 0.0;
double yscale = 0.0;
double zscale = 0.0;
double xscale{ 0.0 };
double yscale{ 0.0 };
double zscale{ 0.0 };
if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
{
if (gradients[i].gradients[j].norm() > 0.0)
Expand Down Expand Up @@ -554,9 +554,9 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con
posed_decompositions.size() + posed_vector_decompositions.size());
return;
}
for (unsigned int i = 0; i < gradients.size(); ++i)
for (unsigned int i{ 0 }; i < gradients.size(); ++i)
{
for (unsigned int j = 0; j < gradients[i].types.size(); ++j)
for (unsigned int j{ 0 }; j < gradients[i].types.size(); ++j)
{
visualization_msgs::msg::Marker sphere_mark;
sphere_mark.type = visualization_msgs::msg::Marker::SPHERE;
Expand Down
Loading