Skip to content

Commit 74debfd

Browse files
tylerjwtylerjw
authored
Node logging in moveit_core (#2503)
1 parent 7ab329d commit 74debfd

File tree

155 files changed

+1327
-1133
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

155 files changed

+1327
-1133
lines changed

‎doc/MIGRATION_GUIDE.md

Lines changed: 3 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -13,29 +13,15 @@ However because of initaliziation order (child logger has to be created after rc
1313
#include <moveit/utils/logger.hpp>
1414
```
1515

16-
Wherever the node is created there is the option to set the global logger for moveit using this syntax:
16+
Wherever the node is created there is the option to set the root logging namespace to be from the node for moveit using this syntax:
1717

1818
```C++
19-
moveit::setLogger(node->get_logger());
19+
moveit::setNodeLoggerName(node->get_name());
2020
```
2121
2222
Then wherever you call a logging macro you can use the `moveit::getLogger()` function:
2323
```C++
24-
RCLCPP_INFO(moveit::getLogger(), "Very important info message");
25-
```
26-
27-
To have namespaces you need to create a child logger.
28-
There is a function for that too.
29-
This creates a child of the global logger.
30-
You'll find this in the constructor of many of our classes.
31-
32-
```C++
33-
, logger_(moveit::makeChildLogger("servo"))
34-
```
35-
36-
Once you have a child logger you can use it in logging macros:
37-
```C++
38-
RCLCPP_INFO(logger_, "Very important info message");
24+
RCLCPP_INFO(moveit::getLogger("my_namespace"), "Very important info message");
3925
```
4026

4127
### Logger naming convention

‎moveit_core/collision_detection/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@ set_target_properties(moveit_collision_detection PROPERTIES VERSION ${${PROJECT_
3535

3636
target_link_libraries(moveit_collision_detection
3737
moveit_robot_state
38+
moveit_utils
3839
)
3940

4041
# unit tests

‎moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp

Lines changed: 48 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -38,103 +38,111 @@
3838
#include <moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h>
3939
#include <rclcpp/logger.hpp>
4040
#include <rclcpp/logging.hpp>
41+
#include <moveit/utils/logger.hpp>
4142

42-
// Logger
43-
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_world_allvalid");
43+
namespace collision_detection
44+
{
45+
namespace
46+
{
47+
rclcpp::Logger getLogger()
48+
{
49+
return moveit::getLogger("collision_detection.world_allvalid");
50+
}
51+
} // namespace
4452

45-
const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID");
53+
const std::string CollisionDetectorAllocatorAllValid::NAME("ALL_VALID");
4654

47-
collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model,
48-
double padding, double scale)
55+
CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding,
56+
double scale)
4957
: CollisionEnv(robot_model, padding, scale)
5058
{
5159
}
5260

53-
collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model,
54-
const WorldPtr& world, double padding, double scale)
61+
CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world,
62+
double padding, double scale)
5563
: CollisionEnv(robot_model, world, padding, scale)
5664
{
5765
}
5866

59-
collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world)
67+
CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world)
6068
: CollisionEnv(other, world)
6169
{
6270
}
6371

64-
void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
65-
const moveit::core::RobotState& /*state*/) const
72+
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
73+
const moveit::core::RobotState& /*state*/) const
6674
{
6775
res.collision = false;
6876
if (req.verbose)
69-
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
77+
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
7078
}
7179

72-
void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
73-
const moveit::core::RobotState& /*state*/,
74-
const AllowedCollisionMatrix& /*acm*/) const
80+
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
81+
const moveit::core::RobotState& /*state*/,
82+
const AllowedCollisionMatrix& /*acm*/) const
7583
{
7684
res.collision = false;
7785
if (req.verbose)
78-
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
86+
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
7987
}
8088

81-
void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
82-
const moveit::core::RobotState& /*state1*/,
83-
const moveit::core::RobotState& /*state2*/) const
89+
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
90+
const moveit::core::RobotState& /*state1*/,
91+
const moveit::core::RobotState& /*state2*/) const
8492
{
8593
res.collision = false;
8694
if (req.verbose)
87-
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
95+
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
8896
}
8997

90-
void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
91-
const moveit::core::RobotState& /*state1*/,
92-
const moveit::core::RobotState& /*state2*/,
93-
const AllowedCollisionMatrix& /*acm*/) const
98+
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
99+
const moveit::core::RobotState& /*state1*/,
100+
const moveit::core::RobotState& /*state2*/,
101+
const AllowedCollisionMatrix& /*acm*/) const
94102
{
95103
res.collision = false;
96104
if (req.verbose)
97-
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
105+
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
98106
}
99107

100-
void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_detection::DistanceRequest& /*req*/,
101-
collision_detection::DistanceResult& res,
102-
const moveit::core::RobotState& /*state*/) const
108+
void CollisionEnvAllValid::distanceRobot(const DistanceRequest& /*req*/, DistanceResult& res,
109+
const moveit::core::RobotState& /*state*/) const
103110
{
104111
res.collision = false;
105112
}
106113

107-
double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/) const
114+
double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/) const
108115
{
109116
return 0.0;
110117
}
111118

112-
double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/,
113-
const AllowedCollisionMatrix& /*acm*/) const
119+
double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/,
120+
const AllowedCollisionMatrix& /*acm*/) const
114121
{
115122
return 0.0;
116123
}
117124

118-
void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
119-
const moveit::core::RobotState& /*state*/) const
125+
void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
126+
const moveit::core::RobotState& /*state*/) const
120127
{
121128
res.collision = false;
122129
if (req.verbose)
123-
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
130+
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
124131
}
125132

126-
void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
127-
const moveit::core::RobotState& /*state*/,
128-
const AllowedCollisionMatrix& /*acm*/) const
133+
void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
134+
const moveit::core::RobotState& /*state*/,
135+
const AllowedCollisionMatrix& /*acm*/) const
129136
{
130137
res.collision = false;
131138
if (req.verbose)
132-
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
139+
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
133140
}
134141

135-
void collision_detection::CollisionEnvAllValid::distanceSelf(const collision_detection::DistanceRequest& /*req*/,
136-
collision_detection::DistanceResult& res,
137-
const moveit::core::RobotState& /*state*/) const
142+
void CollisionEnvAllValid::distanceSelf(const DistanceRequest& /*req*/, DistanceResult& res,
143+
const moveit::core::RobotState& /*state*/) const
138144
{
139145
res.collision = false;
140146
}
147+
148+
} // namespace collision_detection

‎moveit_core/collision_detection/src/collision_common.cpp

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -36,29 +36,37 @@
3636
#include <rclcpp/clock.hpp>
3737
#include <rclcpp/logger.hpp>
3838
#include <rclcpp/logging.hpp>
39+
#include <moveit/utils/logger.hpp>
3940

40-
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_common");
4141
constexpr size_t LOG_THROTTLE_PERIOD{ 5 };
4242

4343
namespace collision_detection
4444
{
45+
namespace
46+
{
47+
rclcpp::Logger getLogger()
48+
{
49+
return moveit::getLogger("collision_detection_common");
50+
}
51+
} // namespace
52+
4553
void CollisionResult::print() const
4654
{
4755
rclcpp::Clock clock;
4856
if (!contacts.empty())
4957
{
5058
#pragma GCC diagnostic push
5159
#pragma GCC diagnostic ignored "-Wold-style-cast"
52-
RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD,
60+
RCLCPP_WARN_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD,
5361
"Objects in collision (printing 1st of "
5462
<< contacts.size() << " pairs): " << contacts.begin()->first.first << ", "
5563
<< contacts.begin()->first.second);
5664

5765
// Log all collisions at the debug level
58-
RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD, "Objects in collision:");
66+
RCLCPP_DEBUG_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD, "Objects in collision:");
5967
for (const auto& contact : contacts)
6068
{
61-
RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD,
69+
RCLCPP_DEBUG_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD,
6270
"\t" << contact.first.first << ", " << contact.first.second);
6371
}
6472
#pragma GCC diagnostic pop

‎moveit_core/collision_detection/src/collision_env.cpp

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -38,20 +38,26 @@
3838
#include <rclcpp/logger.hpp>
3939
#include <rclcpp/logging.hpp>
4040
#include <limits>
41+
#include <moveit/utils/logger.hpp>
4142

42-
// Logger
43-
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_robot");
43+
namespace
44+
{
45+
rclcpp::Logger getLogger()
46+
{
47+
return moveit::getLogger("collision_detection_env");
48+
}
49+
} // namespace
4450

4551
static inline bool validateScale(const double scale)
4652
{
4753
if (scale < std::numeric_limits<double>::epsilon())
4854
{
49-
RCLCPP_ERROR(LOGGER, "Scale must be positive");
55+
RCLCPP_ERROR(getLogger(), "Scale must be positive");
5056
return false;
5157
}
5258
if (scale > std::numeric_limits<double>::max())
5359
{
54-
RCLCPP_ERROR(LOGGER, "Scale must be finite");
60+
RCLCPP_ERROR(getLogger(), "Scale must be finite");
5561
return false;
5662
}
5763
return true;
@@ -61,12 +67,12 @@ static inline bool validatePadding(const double padding)
6167
{
6268
if (padding < 0.0)
6369
{
64-
RCLCPP_ERROR(LOGGER, "Padding cannot be negative");
70+
RCLCPP_ERROR(getLogger(), "Padding cannot be negative");
6571
return false;
6672
}
6773
if (padding > std::numeric_limits<double>::max())
6874
{
69-
RCLCPP_ERROR(LOGGER, "Padding must be finite");
75+
RCLCPP_ERROR(getLogger(), "Padding must be finite");
7076
return false;
7177
}
7278
return true;

‎moveit_core/collision_detection/src/collision_matrix.cpp

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -39,11 +39,17 @@
3939
#include <rclcpp/logging.hpp>
4040
#include <functional>
4141
#include <iomanip>
42+
#include <moveit/utils/logger.hpp>
4243

4344
namespace collision_detection
4445
{
45-
// Logger
46-
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_matrix");
46+
namespace
47+
{
48+
rclcpp::Logger getLogger()
49+
{
50+
return moveit::getLogger("collision_detection_matrix");
51+
}
52+
} // namespace
4753

4854
AllowedCollisionMatrix::AllowedCollisionMatrix()
4955
{
@@ -76,7 +82,8 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo
7682
if (msg.entry_names.size() != msg.entry_values.size() ||
7783
msg.default_entry_names.size() != msg.default_entry_values.size())
7884
{
79-
RCLCPP_ERROR(LOGGER, "The number of links does not match the number of entries in AllowedCollisionMatrix message");
85+
RCLCPP_ERROR(getLogger(),
86+
"The number of links does not match the number of entries in AllowedCollisionMatrix message");
8087
return;
8188
}
8289
for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
@@ -88,7 +95,7 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo
8895
{
8996
if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
9097
{
91-
RCLCPP_ERROR(LOGGER, "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
98+
RCLCPP_ERROR(getLogger(), "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
9299
msg.entry_names[i].c_str());
93100
return;
94101
}

‎moveit_core/collision_detection/src/collision_octomap_filter.cpp

Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -43,9 +43,15 @@
4343
#include <rclcpp/logger.hpp>
4444
#include <rclcpp/logging.hpp>
4545
#include <memory>
46+
#include <moveit/utils/logger.hpp>
4647

47-
// Logger
48-
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_octomap_filter");
48+
namespace
49+
{
50+
rclcpp::Logger getLogger()
51+
{
52+
return moveit::getLogger("collision_detection_octomap_filter");
53+
}
54+
} // namespace
4955

5056
// forward declarations
5157
bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value,
@@ -64,12 +70,12 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
6470
{
6571
if (!object)
6672
{
67-
RCLCPP_ERROR(LOGGER, "No valid Object passed in, cannot refine Normals!");
73+
RCLCPP_ERROR(getLogger(), "No valid Object passed in, cannot refine Normals!");
6874
return 0;
6975
}
7076
if (res.contact_count < 1)
7177
{
72-
RCLCPP_WARN(LOGGER, "There do not appear to be any contacts, so there is nothing to refine!");
78+
RCLCPP_WARN(getLogger(), "There do not appear to be any contacts, so there is nothing to refine!");
7379
return 0;
7480
}
7581

@@ -129,16 +135,16 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
129135
{
130136
count++;
131137
node_centers.push_back(pt);
132-
// RCLCPP_INFO(LOGGER, "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
138+
// RCLCPP_INFO(getLogger(), "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
133139
// count, prob, pt.x(), pt.y(), pt.z());
134140
}
135141
}
136-
// RCLCPP_INFO(LOGGER, "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells
142+
// RCLCPP_INFO(getLogger(), "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells
137143
// %d",
138144
// contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);
139145

140146
// octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
141-
// RCLCPP_ERROR(LOGGER, "bad stuff in collision_octomap_filter.cpp; need to port octomap
147+
// RCLCPP_ERROR(getLogger(), "bad stuff in collision_octomap_filter.cpp; need to port octomap
142148
// call for groovy");
143149

144150
octomath::Vector3 n;
@@ -151,7 +157,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
151157
if (divergence > allowed_angle_divergence)
152158
{
153159
modified++;
154-
// RCLCPP_INFO(LOGGER, "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f,
160+
// RCLCPP_INFO(getLogger(), "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f,
155161
// %.3f, %.3f]",
156162
// divergence, contact_normal.x(), contact_normal.y(), contact_normal.z(),
157163
// n.x(), n.y(), n.z());
@@ -268,7 +274,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_mu
268274
}
269275
else
270276
{
271-
RCLCPP_ERROR(LOGGER, "This should not be called!");
277+
RCLCPP_ERROR(getLogger(), "This should not be called!");
272278
}
273279

274280
double f_val = 0;
@@ -294,7 +300,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_mu
294300
}
295301
else
296302
{
297-
RCLCPP_ERROR(LOGGER, "This should not be called!");
303+
RCLCPP_ERROR(getLogger(), "This should not be called!");
298304
const double r_scaled = r / r;
299305
// TODO still need to address the scaling...
300306
f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1);

0 commit comments

Comments
 (0)