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
#9848)

feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files control/autoware_pid_longitudinal_controller

Signed-off-by: vish0012 <[email protected]>
  • Loading branch information
vish0012 authored Jan 9, 2025
1 parent 4d0a86f commit 926ad7f
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -34,11 +34,11 @@

#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
#include "autoware_control_msgs/msg/longitudinal.hpp"
#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
#include "visualization_msgs/msg/marker.hpp"

#include <deque>
Expand Down Expand Up @@ -98,8 +98,10 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_;
// ros variables
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_slope;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug;
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr
m_pub_slope;
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr
m_pub_debug;
rclcpp::Publisher<MarkerArray>::SharedPtr m_pub_virtual_wall_marker;

rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
Expand Down
2 changes: 1 addition & 1 deletion control/autoware_pid_longitudinal_controller/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>autoware_internal_debug_msgs</depend>
<depend>autoware_interpolation</depend>
<depend>autoware_motion_utils</depend>
<depend>autoware_planning_msgs</depend>
Expand All @@ -37,7 +38,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
Original file line number Diff line number Diff line change
Expand Up @@ -214,9 +214,9 @@ PidLongitudinalController::PidLongitudinalController(
: node.declare_parameter<double>("ego_nearest_yaw_threshold"); // [rad]

// subscriber, publisher
m_pub_slope = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
m_pub_slope = node.create_publisher<autoware_internal_debug_msgs::msg::Float32MultiArrayStamped>(
"~/output/slope_angle", rclcpp::QoS{1});
m_pub_debug = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
m_pub_debug = node.create_publisher<autoware_internal_debug_msgs::msg::Float32MultiArrayStamped>(
"~/output/longitudinal_diagnostic", rclcpp::QoS{1});
m_pub_virtual_wall_marker = node.create_publisher<MarkerArray>("~/virtual_wall", 1);

Expand Down Expand Up @@ -931,15 +931,15 @@ void PidLongitudinalController::publishDebugData(
m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc);

// publish debug values
tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{};
autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{};
debug_msg.stamp = clock_->now();
for (const auto & v : m_debug_values.getValues()) {
debug_msg.data.push_back(static_cast<decltype(debug_msg.data)::value_type>(v));
}
m_pub_debug->publish(debug_msg);

// slope angle
tier4_debug_msgs::msg::Float32MultiArrayStamped slope_msg{};
autoware_internal_debug_msgs::msg::Float32MultiArrayStamped slope_msg{};
slope_msg.stamp = clock_->now();
slope_msg.data.push_back(
static_cast<decltype(slope_msg.data)::value_type>(control_data.slope_angle));
Expand Down

0 comments on commit 926ad7f

Please sign in to comment.