Skip to content

Commit 5916006

Browse files
author
Ahmed Ebrahim
committed
fix(log-messages): fixing info and debug messages
Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>
1 parent 02b56ea commit 5916006

File tree

15 files changed

+39
-39
lines changed

15 files changed

+39
-39
lines changed

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -233,28 +233,28 @@ void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstShar
233233
bool LaneDepartureCheckerNode::isDataReady()
234234
{
235235
if (!current_odom_) {
236-
RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_twist msg...");
236+
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_twist msg...");
237237
return false;
238238
}
239239

240240
if (!lanelet_map_) {
241-
RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for lanelet_map msg...");
241+
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for lanelet_map msg...");
242242
return false;
243243
}
244244

245245
if (!route_) {
246-
RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route msg...");
246+
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route msg...");
247247
return false;
248248
}
249249

250250
if (!reference_trajectory_) {
251-
RCLCPP_DEBUG_THROTTLE(
251+
RCLCPP_INFO_THROTTLE(
252252
get_logger(), *get_clock(), 5000, "waiting for reference_trajectory msg...");
253253
return false;
254254
}
255255

256256
if (!predicted_trajectory_) {
257-
RCLCPP_DEBUG_THROTTLE(
257+
RCLCPP_INFO_THROTTLE(
258258
get_logger(), *get_clock(), 5000, "waiting for predicted_trajectory msg...");
259259
return false;
260260
}

control/trajectory_follower_node/src/controller_node.cpp

+6-6
Original file line numberDiff line numberDiff line change
@@ -154,27 +154,27 @@ boost::optional<trajectory_follower::InputData> Controller::createInputData(
154154
rclcpp::Clock & clock) const
155155
{
156156
if (!current_trajectory_ptr_) {
157-
RCLCPP_DEBUG_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory.");
157+
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory.");
158158
return {};
159159
}
160160

161161
if (!current_odometry_ptr_) {
162-
RCLCPP_DEBUG_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry.");
162+
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry.");
163163
return {};
164164
}
165165

166166
if (!current_steering_ptr_) {
167-
RCLCPP_DEBUG_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering.");
167+
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering.");
168168
return {};
169169
}
170170

171171
if (!current_accel_ptr_) {
172-
RCLCPP_DEBUG_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel.");
172+
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel.");
173173
return {};
174174
}
175175

176176
if (!current_operation_mode_ptr_) {
177-
RCLCPP_DEBUG_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode.");
177+
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode.");
178178
return {};
179179
}
180180

@@ -193,7 +193,7 @@ void Controller::callbackTimerControl()
193193
// 1. create input data
194194
const auto input_data = createInputData(*get_clock());
195195
if (!input_data) {
196-
RCLCPP_DEBUG_THROTTLE(
196+
RCLCPP_INFO_THROTTLE(
197197
get_logger(), *get_clock(), 5000, "Control is skipped since input data is not ready.");
198198
return;
199199
}

map/map_tf_generator/src/vector_map_tf_generator_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node
8888

8989
static_broadcaster_->sendTransform(static_transformStamped);
9090

91-
RCLCPP_DEBUG_STREAM(
91+
RCLCPP_INFO_STREAM(
9292
get_logger(), "broadcast static tf. map_frame:"
9393
<< map_frame_ << ", viewer_frame:" << viewer_frame_ << ", x:" << coordinate_x
9494
<< ", y:" << coordinate_y << ", z:" << coordinate_z);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -270,7 +270,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
270270
bool BehaviorPathPlannerNode::isDataReady()
271271
{
272272
const auto missing = [this](const auto & name) {
273-
RCLCPP_DEBUG_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", name);
273+
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", name);
274274
return false;
275275
};
276276

planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -189,7 +189,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
189189
tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
190190
break;
191191
} catch (const tf2::TransformException & ex) {
192-
RCLCPP_DEBUG(this->get_logger(), "waiting for initial pose...");
192+
RCLCPP_INFO(this->get_logger(), "waiting for initial pose...");
193193
}
194194
rclcpp::sleep_for(std::chrono::milliseconds(5000));
195195
}

planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -372,7 +372,7 @@ bool FreespacePlannerNode::isPlanRequired()
372372
const bool is_obstacle_found =
373373
algo_->hasObstacleOnTrajectory(trajectory2PoseArray(forward_trajectory));
374374
if (is_obstacle_found) {
375-
RCLCPP_INFO(get_logger(), "Found obstacle");
375+
RCLCPP_DEBUG(get_logger(), "Found obstacle");
376376
return true;
377377
}
378378
}
@@ -493,10 +493,10 @@ void FreespacePlannerNode::planTrajectory()
493493
const bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame);
494494
const rclcpp::Time end = get_clock()->now();
495495

496-
RCLCPP_INFO(get_logger(), "Freespace planning: %f [s]", (end - start).seconds());
496+
RCLCPP_DEBUG(get_logger(), "Freespace planning: %f [s]", (end - start).seconds());
497497

498498
if (result) {
499-
RCLCPP_INFO(get_logger(), "Found goal!");
499+
RCLCPP_DEBUG(get_logger(), "Found goal!");
500500
trajectory_ =
501501
createTrajectory(current_pose_, algo_->getWaypoints(), node_param_.waypoints_velocity);
502502
reversing_indices_ = getReversingIndices(trajectory_);

planning/mission_planner/src/mission_planner/mission_planner.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -133,12 +133,12 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
133133
void MissionPlanner::checkInitialization()
134134
{
135135
if (!planner_->ready()) {
136-
RCLCPP_DEBUG_THROTTLE(
136+
RCLCPP_INFO_THROTTLE(
137137
get_logger(), *get_clock(), 5000, "waiting lanelet map... Route API is not ready.");
138138
return;
139139
}
140140
if (!odometry_) {
141-
RCLCPP_DEBUG_THROTTLE(
141+
RCLCPP_INFO_THROTTLE(
142142
get_logger(), *get_clock(), 5000, "waiting odometry... Route API is not ready.");
143143
return;
144144
}

planning/obstacle_avoidance_planner/src/replan_checker.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -65,22 +65,22 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const
6565

6666
// path shape changes
6767
if (isPathAroundEgoChanged(planner_data, prev_traj_points)) {
68-
RCLCPP_INFO(
68+
RCLCPP_DEBUG(
6969
logger_, "Replan with resetting optimization since path shape around ego changed.");
7070
return true;
7171
}
7272

7373
// path goal changes
7474
if (isPathGoalChanged(planner_data, prev_traj_points)) {
75-
RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed.");
75+
RCLCPP_DEBUG(logger_, "Replan with resetting optimization since path goal changed.");
7676
return true;
7777
}
7878

7979
// ego pose is lost or new ego pose is designated in simulation
8080
const double delta_dist =
8181
tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position);
8282
if (max_ego_moving_dist_ < delta_dist) {
83-
RCLCPP_INFO(
83+
RCLCPP_DEBUG(
8484
logger_,
8585
"Replan with resetting optimization since current ego pose is far from previous ego pose.");
8686
return true;

planning/path_smoother/src/replan_checker.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -67,22 +67,22 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const
6767

6868
// path shape changes
6969
if (isPathAroundEgoChanged(planner_data, prev_traj_points)) {
70-
RCLCPP_INFO(
70+
RCLCPP_DEBUG(
7171
logger_, "Replan with resetting optimization since path shape around ego changed.");
7272
return true;
7373
}
7474

7575
// path goal changes
7676
if (isPathGoalChanged(planner_data, prev_traj_points)) {
77-
RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed.");
77+
RCLCPP_DEBUG(logger_, "Replan with resetting optimization since path goal changed.");
7878
return true;
7979
}
8080

8181
// ego pose is lost or new ego pose is designated in simulation
8282
const double delta_dist =
8383
tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position);
8484
if (max_ego_moving_dist_ < delta_dist) {
85-
RCLCPP_INFO(
85+
RCLCPP_DEBUG(
8686
logger_,
8787
"Replan with resetting optimization since current ego pose is far from previous ego pose.");
8888
return true;

planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,7 @@ void ScenarioSelectorNode::updateCurrentScenario()
180180
}
181181

182182
if (current_scenario_ != prev_scenario) {
183-
RCLCPP_INFO_STREAM(
183+
RCLCPP_DEBUG_STREAM(
184184
this->get_logger(), "scenario changed: " << prev_scenario << " -> " << current_scenario_);
185185
}
186186
}
@@ -237,23 +237,23 @@ void ScenarioSelectorNode::onParkingState(const std_msgs::msg::Bool::ConstShared
237237
bool ScenarioSelectorNode::isDataReady()
238238
{
239239
if (!current_pose_) {
240-
RCLCPP_DEBUG_THROTTLE(
240+
RCLCPP_INFO_THROTTLE(
241241
this->get_logger(), *this->get_clock(), 5000, "Waiting for current pose.");
242242
return false;
243243
}
244244

245245
if (!lanelet_map_ptr_) {
246-
RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for lanelet map.");
246+
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for lanelet map.");
247247
return false;
248248
}
249249

250250
if (!route_) {
251-
RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for route.");
251+
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for route.");
252252
return false;
253253
}
254254

255255
if (!twist_) {
256-
RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for twist.");
256+
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for twist.");
257257
return false;
258258
}
259259

planning/surround_obstacle_checker/src/node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -299,7 +299,7 @@ void SurroundObstacleCheckerNode::onTimer()
299299
}
300300

301301
if (use_dynamic_object_ && !object_ptr_) {
302-
RCLCPP_DEBUG_THROTTLE(
302+
RCLCPP_INFO_THROTTLE(
303303
this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info...");
304304
}
305305

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -343,7 +343,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const
343343
void SimplePlanningSimulator::on_timer()
344344
{
345345
if (!is_initialized_) {
346-
RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "waiting initialization...");
346+
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting initialization...");
347347
return;
348348
}
349349

system/default_ad_api/src/operation_mode.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -146,7 +146,7 @@ void OperationModeNode::on_timer()
146146
mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available;
147147

148148
if (!unhealthy_components.empty()) {
149-
RCLCPP_DEBUG_THROTTLE(
149+
RCLCPP_INFO_THROTTLE(
150150
get_logger(), *get_clock(), 3000,
151151
"%s component state is unhealthy. Autonomous is not available.",
152152
unhealthy_components.c_str());

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -285,7 +285,7 @@ void EmergencyHandler::cancelMrmBehavior(
285285
bool EmergencyHandler::isDataReady()
286286
{
287287
if (!hazard_status_stamped_) {
288-
RCLCPP_DEBUG_THROTTLE(
288+
RCLCPP_INFO_THROTTLE(
289289
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(),
290290
"waiting for hazard_status_stamped msg...");
291291
return false;
@@ -358,7 +358,7 @@ void EmergencyHandler::transitionTo(const int new_state)
358358
throw std::runtime_error(msg);
359359
};
360360

361-
RCLCPP_INFO(
361+
RCLCPP_DEBUG(
362362
this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state),
363363
state2string(new_state));
364364

system/system_error_monitor/src/system_error_monitor_core.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -389,22 +389,22 @@ void AutowareErrorMonitor::onControlMode(
389389
bool AutowareErrorMonitor::isDataReady()
390390
{
391391
if (!diag_array_) {
392-
RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for diag_array msg...");
392+
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for diag_array msg...");
393393
return false;
394394
}
395395

396396
if (!current_gate_mode_) {
397-
RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_gate_mode msg...");
397+
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_gate_mode msg...");
398398
return false;
399399
}
400400

401401
if (!autoware_state_) {
402-
RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for autoware_state msg...");
402+
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for autoware_state msg...");
403403
return false;
404404
}
405405

406406
if (!control_mode_) {
407-
RCLCPP_DEBUG_THROTTLE(
407+
RCLCPP_INFO_THROTTLE(
408408
get_logger(), *get_clock(), 5000, "waiting for vehicle_state_report msg...");
409409
return false;
410410
}

0 commit comments

Comments
 (0)