Skip to content

Commit

Permalink
Fix clang-tidy wanrings
Browse files Browse the repository at this point in the history
  • Loading branch information
tylerjw committed Dec 1, 2023
1 parent 00819dd commit aa250dc
Show file tree
Hide file tree
Showing 7 changed files with 24 additions and 17 deletions.
4 changes: 4 additions & 0 deletions moveit_core/collision_detection_fcl/src/collision_common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -167,8 +167,10 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
return false;

if (cdata->req_->verbose)
{
RCLCPP_DEBUG(getLogger(), "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 };
Expand Down Expand Up @@ -534,8 +536,10 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
return false;
}
if (cdata->req->verbose)
{
RCLCPP_DEBUG(getLogger(), "Actually checking collisions between %s and %s", cd1->getID().c_str(),
cd2->getID().c_str());
}

double dist_threshold = cdata->req->distance_threshold;

Expand Down
2 changes: 2 additions & 0 deletions moveit_core/robot_state/src/conversions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -359,8 +359,10 @@ void msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::AttachedCol
else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE)
{
if (!state.clearAttachedBody(aco.object.id))
{
RCLCPP_ERROR(getLogger(), "The attached body '%s' can not be removed because it does not exist",
aco.link_name.c_str());
}
}
else
RCLCPP_ERROR(getLogger(), "Unknown collision object operation: %d", aco.object.operation);
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/utils/include/moveit/utils/moveit_error_code.h
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes
@param error_code Error code to be translated to a string
@return Error code string
*/
inline std::string errorCodeToString(MoveItErrorCode error_code)
inline std::string errorCodeToString(const MoveItErrorCode& error_code)
{
switch (error_code.val)
{
Expand Down
8 changes: 4 additions & 4 deletions moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,9 +58,10 @@
#include <sstream>

static void publishOctomap(const rclcpp::Publisher<octomap_msgs::msg::Octomap>::SharedPtr& octree_binary_pub,
occupancy_map_monitor::OccupancyMapMonitor& server, rclcpp::Logger logger)
occupancy_map_monitor::OccupancyMapMonitor& server)
{
octomap_msgs::msg::Octomap map;
const auto logger = moveit::getLogger("occupancy_map_monitor");

map.header.frame_id = server.getMapFrame();
map.header.stamp = rclcpp::Clock().now();
Expand Down Expand Up @@ -100,9 +101,8 @@ int main(int argc, char** argv)
std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>(clock_ptr, tf2::durationFromSec(5.0));
tf2_ros::TransformListener listener(*buffer, node, false /* spin_thread - disables executor */);
occupancy_map_monitor::OccupancyMapMonitor server(node, buffer);
server.setUpdateCallback([&octree_binary_pub, &server, logger = node->get_logger()] {
return publishOctomap(octree_binary_pub, server, logger);
});
server.setUpdateCallback(
[&octree_binary_pub, &server, logger = node->get_logger()] { return publishOctomap(octree_binary_pub, server); });
server.startMonitor();

rclcpp::spin(node);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,9 @@ namespace moveit
namespace planning_pipeline_interfaces
{

rclcpp::Logger get_logger()
rclcpp::Logger getLogger()
{
static rclcpp::Logger logger = moveit::getLogger("planning_pipeline_interfaces");
return logger;
return moveit::getLogger("planning_pipeline_interfaces");
}

::planning_interface::MotionPlanResponse
Expand All @@ -59,7 +58,7 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla
auto it = planning_pipelines.find(motion_plan_request.pipeline_id);
if (it == planning_pipelines.end())
{
RCLCPP_ERROR(get_logger(), "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str());
RCLCPP_ERROR(getLogger(), "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str());
motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE;
return motion_plan_response;
}
Expand Down Expand Up @@ -88,7 +87,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe
const auto hardware_concurrency = std::thread::hardware_concurrency();
if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0)
{
RCLCPP_WARN(get_logger(),
RCLCPP_WARN(getLogger(),
"More parallel planning problems defined ('%ld') than possible to solve concurrently with the "
"hardware ('%d')",
motion_plan_requests.size(), hardware_concurrency);
Expand All @@ -106,7 +105,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe
}
catch (const std::exception& e)
{
RCLCPP_ERROR(get_logger(), "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what());
RCLCPP_ERROR(getLogger(), "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what());
plan_solution = ::planning_interface::MotionPlanResponse();
plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE;
}
Expand All @@ -118,7 +117,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe
if (stopping_criterion_callback(plan_responses_container, motion_plan_requests))
{
// Terminate planning pipelines
RCLCPP_INFO(get_logger(), "Stopping criterion met: Terminating planning pipelines that are still active");
RCLCPP_INFO(getLogger(), "Stopping criterion met: Terminating planning pipelines that are still active");
for (const auto& request : motion_plan_requests)
{
try
Expand All @@ -131,7 +130,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe
}
catch (const std::out_of_range&)
{
RCLCPP_WARN(get_logger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists",
RCLCPP_WARN(getLogger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists",
request.pipeline_id.c_str());
}
}
Expand Down Expand Up @@ -174,7 +173,7 @@ createPlanningPipelineMap(const std::vector<std::string>& pipeline_names,
// Check if pipeline already exists
if (planning_pipelines.count(planning_pipeline_name) > 0)
{
RCLCPP_WARN(get_logger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str());
RCLCPP_WARN(getLogger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str());
continue;
}

Expand All @@ -184,7 +183,7 @@ createPlanningPipelineMap(const std::vector<std::string>& pipeline_names,

if (!pipeline->getPlannerManager())
{
RCLCPP_ERROR(get_logger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
continue;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1341,7 +1341,9 @@ MoveGroupInterface::~MoveGroupInterface()
}

MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) noexcept
: remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_), logger_(other.logger_)
: remembered_joint_values_(std::move(other.remembered_joint_values_))
, impl_(other.impl_)
, logger_(std::move(other.logger_))
{
other.impl_ = nullptr;
}
Expand Down
2 changes: 1 addition & 1 deletion moveit_ros/warehouse/src/save_as_text.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ typedef std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Quaternion> Lin
typedef std::map<std::string, LinkConstraintPair> LinkConstraintMap;

void collectLinkConstraints(const moveit_msgs::msg::Constraints& constraints, LinkConstraintMap& lcmap,
rclcpp::Logger logger)
const rclcpp::Logger& logger)
{
for (const moveit_msgs::msg::PositionConstraint& position_constraint : constraints.position_constraints)
{
Expand Down

0 comments on commit aa250dc

Please sign in to comment.