diff --git a/AutowareAuto/src/control/trajectory_follower_nodes/src/lateral_controller_node.cpp b/AutowareAuto/src/control/trajectory_follower_nodes/src/lateral_controller_node.cpp index 3f6f8132..50a4d9ad 100644 --- a/AutowareAuto/src/control/trajectory_follower_nodes/src/lateral_controller_node.cpp +++ b/AutowareAuto/src/control/trajectory_follower_nodes/src/lateral_controller_node.cpp @@ -87,7 +87,6 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options) const float64_t cg_to_front_m = declare_parameter("vehicle.cg_to_front_m").get(); const float64_t cg_to_rear_m = declare_parameter("vehicle.cg_to_rear_m").get(); const float64_t wheelbase = cg_to_front_m + cg_to_rear_m; - RCLCPP_ERROR(get_logger(), "wheelbase: %f", wheelbase); /* vehicle model setup */ const std::string vehicle_model_type = declare_parameter("vehicle_model_type").get(); @@ -210,7 +209,7 @@ void LateralController::onTimer() m_current_pose_ptr->pose, ctrl_cmd, predicted_traj, diagnostic); if (isStoppedState()) { - RCLCPP_ERROR(get_logger(), "isStoppedState"); + RCLCPP_DEBUG(get_logger(), "isStoppedState"); // Reset input buffer for (auto & value : m_mpc.m_input_buffer) { value = m_ctrl_cmd_prev.steering_tire_angle; @@ -230,8 +229,6 @@ void LateralController::onTimer() ctrl_cmd = getStopControlCommand(); } - RCLCPP_ERROR(get_logger(), "MPC solved"); - m_ctrl_cmd_prev = ctrl_cmd; publishCtrlCmd(ctrl_cmd); publishPredictedTraj(predicted_traj); @@ -241,25 +238,25 @@ void LateralController::onTimer() bool8_t LateralController::checkData() const { if (!m_mpc.hasVehicleModel()) { - RCLCPP_ERROR( + RCLCPP_DEBUG( get_logger(), "MPC does not have a vehicle model"); return false; } if (!m_mpc.hasQPSolver()) { - RCLCPP_ERROR( + RCLCPP_DEBUG( get_logger(), "MPC does not have a QP solver"); return false; } if (!m_current_pose_ptr || !m_current_state_ptr) { - RCLCPP_ERROR( + RCLCPP_DEBUG( get_logger(), "waiting data. pose = %d, current_state = %d", m_current_pose_ptr != nullptr, m_current_state_ptr != nullptr); return false; } if (m_mpc.m_ref_traj.size() == 0) { - RCLCPP_ERROR(get_logger(), "trajectory size is zero."); + RCLCPP_DEBUG(get_logger(), "trajectory size is zero."); return false; } @@ -271,7 +268,7 @@ void LateralController::onTrajectory(const autoware_auto_msgs::msg::Trajectory:: m_current_trajectory_ptr = msg; if (msg->points.size() < 3) { - RCLCPP_ERROR(get_logger(), "received path size is < 3, not enough."); + RCLCPP_DEBUG(get_logger(), "received path size is < 3, not enough."); return; } @@ -303,8 +300,6 @@ void LateralController::updateCurrentPose() ps.pose.position.y = transform.transform.translation.y; ps.pose.position.z = transform.transform.translation.z; ps.pose.orientation = transform.transform.rotation; - RCLCPP_ERROR(get_logger(), "ps.pose.position.x : %f" , ps.pose.position.x); - RCLCPP_ERROR(get_logger(), "ps.pose.position.y : %f" , ps.pose.position.y); m_current_pose_ptr = std::make_shared(ps); } @@ -336,7 +331,6 @@ bool8_t LateralController::isStoppedState() const m_current_pose_ptr->pose); // If the nearest index is not found, return false - RCLCPP_ERROR(get_logger(), "nearest index %d", nearest); if (nearest < 0) { RCLCPP_ERROR(get_logger(), "nearest index is not found"); @@ -346,13 +340,13 @@ bool8_t LateralController::isStoppedState() const *m_current_trajectory_ptr, nearest); if (dist < m_stop_state_keep_stopping_dist) { - RCLCPP_ERROR( + RCLCPP_DEBUG( get_logger(), "stop_dist = %f < %f : m_stop_state_keep_stopping_dist. keep stopping.", dist, m_stop_state_keep_stopping_dist); return true; } - RCLCPP_ERROR(get_logger(), "stop_dist = %f release stopping.", dist); + RCLCPP_DEBUG(get_logger(), "stop_dist = %f release stopping.", dist); const float64_t current_vel = m_current_state_ptr->state.longitudinal_velocity_mps; const float64_t target_vel = @@ -371,7 +365,6 @@ void LateralController::publishCtrlCmd(autoware_auto_msgs::msg::AckermannLateral { ctrl_cmd.stamp = this->now(); m_pub_ctrl_cmd->publish(ctrl_cmd); - RCLCPP_ERROR(get_logger(), "published command = %f radian", static_cast(ctrl_cmd.steering_tire_angle)); m_steer_cmd_prev = ctrl_cmd.steering_tire_angle; } diff --git a/AutowareAuto/src/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/AutowareAuto/src/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index 6f11e50f..19ab8d06 100644 --- a/AutowareAuto/src/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/AutowareAuto/src/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -390,12 +390,10 @@ void LongitudinalController::callbackTimerControl() const auto control_data = getControlData(current_pose); - RCLCPP_ERROR(get_logger(), "current_pose.position.x : %f" , current_pose.position.x); - RCLCPP_ERROR(get_logger(), "current_pose.position.y : %f" , current_pose.position.y); // self pose is far from trajectory if (control_data.is_far_from_trajectory) { - RCLCPP_ERROR(get_logger(), "self pose is far from trajectory."); + RCLCPP_DEBUG(get_logger(), "self pose is far from trajectory."); m_control_state = ControlState::EMERGENCY; // update control state const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command m_prev_raw_ctrl_cmd = raw_ctrl_cmd; @@ -589,7 +587,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( raw_ctrl_cmd.vel = target_motion.vel; raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); - RCLCPP_ERROR( + RCLCPP_DEBUG( get_logger(), "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " "feedback_ctrl_cmd.ac: %3.3f", @@ -600,7 +598,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( control_data.stop_dist, current_vel, current_acc, m_vel_hist, m_delay_compensation_time); raw_ctrl_cmd.vel = m_stopped_state_params.vel; - RCLCPP_ERROR( + RCLCPP_DEBUG( get_logger(), "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); @@ -616,7 +614,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); - RCLCPP_ERROR( + RCLCPP_DEBUG( get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); } else if (current_control_state == ControlState::EMERGENCY) {