Skip to content

Commit

Permalink
feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (
Browse files Browse the repository at this point in the history
#9846)

* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files ontrol/autoware_mpc_lateral_controller

Signed-off-by: vish0012 <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: vish0012 <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
vish0012 and pre-commit-ci[bot] authored Jan 9, 2025
1 parent 926ad7f commit 0b73c13
Show file tree
Hide file tree
Showing 7 changed files with 20 additions and 17 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -24,14 +24,14 @@
#include <rclcpp/rclcpp.hpp>

#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <autoware_planning_msgs/msg/trajectory.hpp>
#include <autoware_vehicle_msgs/msg/control_mode_report.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_debug_msgs/msg/float64_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <lanelet2_core/LaneletMap.h>
Expand Down Expand Up @@ -115,7 +115,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node
autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"};
autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
this, "~/debug/processing_time_ms_diag"};
rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
processing_time_publisher_;

// Timer
rclcpp::TimerBase::SharedPtr timer_;
Expand Down
2 changes: 1 addition & 1 deletion control/autoware_lane_departure_checker/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_internal_debug_msgs</depend>
<depend>autoware_lanelet2_extension</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_motion_utils</depend>
Expand All @@ -32,7 +33,6 @@
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -171,7 +171,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o

// Publisher
processing_time_publisher_ =
this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
this->create_publisher<autoware_internal_debug_msgs::msg::Float64Stamped>(
"~/debug/processing_time_ms", 1);
// Nothing

// Diagnostic Updater
Expand Down Expand Up @@ -342,10 +343,11 @@ void LaneDepartureCheckerNode::onTimer()

{
const auto & deviation = output_.trajectory_deviation;
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
debug_publisher_.publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
"deviation/lateral", deviation.lateral);
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>("deviation/yaw", deviation.yaw);
debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
debug_publisher_.publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
"deviation/yaw", deviation.yaw);
debug_publisher_.publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
"deviation/yaw_deg", rad2deg(deviation.yaw));
}
processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true);
Expand All @@ -361,7 +363,7 @@ void LaneDepartureCheckerNode::onTimer()

processing_time_map["Total"] = stop_watch.toc("Total");
processing_diag_publisher_.publish(processing_time_map);
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = get_clock()->now();
processing_time_msg.data = processing_time_map["Total"];
processing_time_publisher_->publish(processing_time_msg);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,11 @@
#include "rclcpp/rclcpp.hpp"

#include "autoware_control_msgs/msg/lateral.hpp"
#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"

#include <deque>
#include <memory>
Expand All @@ -41,11 +41,11 @@ namespace autoware::motion::control::mpc_lateral_controller

using autoware::motion::control::trajectory_follower::LateralHorizon;
using autoware_control_msgs::msg::Lateral;
using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped;
using autoware_planning_msgs::msg::Trajectory;
using autoware_vehicle_msgs::msg::SteeringReport;
using geometry_msgs::msg::Pose;
using nav_msgs::msg::Odometry;
using tier4_debug_msgs::msg::Float32MultiArrayStamped;

using Eigen::MatrixXd;
using Eigen::VectorXd;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,13 +26,13 @@
#include <diagnostic_updater/diagnostic_updater.hpp>

#include "autoware_control_msgs/msg/lateral.hpp"
#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp"
#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
#include "tier4_debug_msgs/msg/float32_stamped.hpp"

#include <deque>
#include <memory>
Expand All @@ -45,11 +45,11 @@ namespace autoware::motion::control::mpc_lateral_controller

namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
using autoware_control_msgs::msg::Lateral;
using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped;
using autoware_internal_debug_msgs::msg::Float32Stamped;
using autoware_planning_msgs::msg::Trajectory;
using autoware_vehicle_msgs::msg::SteeringReport;
using nav_msgs::msg::Odometry;
using tier4_debug_msgs::msg::Float32MultiArrayStamped;
using tier4_debug_msgs::msg::Float32Stamped;
using trajectory_follower::LateralHorizon;

class MpcLateralController : public trajectory_follower::LateralControllerBase
Expand Down
2 changes: 1 addition & 1 deletion control/autoware_mpc_lateral_controller/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_control_msgs</depend>
<depend>autoware_internal_debug_msgs</depend>
<depend>autoware_interpolation</depend>
<depend>autoware_motion_utils</depend>
<depend>autoware_osqp_interface</depend>
Expand All @@ -36,7 +37,6 @@
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
4 changes: 2 additions & 2 deletions control/autoware_mpc_lateral_controller/test/test_mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,11 @@
#include <autoware/trajectory_follower_base/control_horizon.hpp>

#include "autoware_control_msgs/msg/lateral.hpp"
#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "autoware_planning_msgs/msg/trajectory_point.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"

#ifdef ROS_DISTRO_GALACTIC
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
Expand All @@ -45,12 +45,12 @@ namespace autoware::motion::control::mpc_lateral_controller

using autoware::motion::control::trajectory_follower::LateralHorizon;
using autoware_control_msgs::msg::Lateral;
using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped;
using autoware_planning_msgs::msg::Trajectory;
using autoware_planning_msgs::msg::TrajectoryPoint;
using autoware_vehicle_msgs::msg::SteeringReport;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseStamped;
using tier4_debug_msgs::msg::Float32MultiArrayStamped;

TrajectoryPoint makePoint(const double x, const double y, const float vx)
{
Expand Down

0 comments on commit 0b73c13

Please sign in to comment.