Skip to content

Commit

Permalink
feat(tier4_debug_tools)!: replace tier4_debug_msgs with tier4_interna…
Browse files Browse the repository at this point in the history
…l_debug_msgs

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
  • Loading branch information
mitsudome-r committed Jan 22, 2025
1 parent 93dfb1c commit aba16ba
Show file tree
Hide file tree
Showing 3 changed files with 11 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@

#include <autoware_planning_msgs/msg/trajectory.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <tier4_debug_msgs/msg/float32_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/float32_stamped.hpp>

class LateralErrorPublisher : public rclcpp::Node
{
Expand All @@ -50,11 +50,11 @@ class LateralErrorPublisher : public rclcpp::Node
sub_vehicle_pose_; //!< @brief subscription for vehicle pose
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
sub_ground_truth_pose_; //!< @brief subscription for gnss pose
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
pub_control_lateral_error_; //!< @brief publisher for control lateral error
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
pub_localization_lateral_error_; //!< @brief publisher for localization lateral error
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
pub_lateral_error_; //!< @brief publisher for lateral error (control + localization)

/**
Expand Down
2 changes: 1 addition & 1 deletion common/tier4_debug_tools/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>
<depend>autoware_internal_debug_msgs</depend>

<exec_depend>launch_ros</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-rtree</exec_depend>
Expand Down
12 changes: 6 additions & 6 deletions common/tier4_debug_tools/src/lateral_error_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,11 @@ LateralErrorPublisher::LateralErrorPublisher(const rclcpp::NodeOptions & node_op
"~/input/ground_truth_pose_with_covariance", rclcpp::QoS{1},
std::bind(&LateralErrorPublisher::onGroundTruthPose, this, _1));
pub_control_lateral_error_ =
create_publisher<tier4_debug_msgs::msg::Float32Stamped>("~/control_lateral_error", 1);
create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>("~/control_lateral_error", 1);
pub_localization_lateral_error_ =
create_publisher<tier4_debug_msgs::msg::Float32Stamped>("~/localization_lateral_error", 1);
create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>("~/localization_lateral_error", 1);
pub_lateral_error_ =
create_publisher<tier4_debug_msgs::msg::Float32Stamped>("~/lateral_error", 1);
create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>("~/lateral_error", 1);
}

void LateralErrorPublisher::onTrajectory(
Expand Down Expand Up @@ -131,17 +131,17 @@ void LateralErrorPublisher::onGroundTruthPose(
RCLCPP_DEBUG(this->get_logger(), "localization_error: %f", lateral_error);

// Publish lateral errors
tier4_debug_msgs::msg::Float32Stamped control_msg;
autoware_internal_debug_msgs::msg::Float32Stamped control_msg;
control_msg.stamp = this->now();
control_msg.data = static_cast<float>(control_lateral_error);
pub_control_lateral_error_->publish(control_msg);

tier4_debug_msgs::msg::Float32Stamped localization_msg;
autoware_internal_debug_msgs::msg::Float32Stamped localization_msg;
localization_msg.stamp = this->now();
localization_msg.data = static_cast<float>(localization_lateral_error);
pub_localization_lateral_error_->publish(localization_msg);

tier4_debug_msgs::msg::Float32Stamped sum_msg;
autoware_internal_debug_msgs::msg::Float32Stamped sum_msg;
sum_msg.stamp = this->now();
sum_msg.data = static_cast<float>(lateral_error);
pub_lateral_error_->publish(sum_msg);
Expand Down

0 comments on commit aba16ba

Please sign in to comment.