Skip to content

Commit

Permalink
feat(deviation_estimation_tools)!: replace tier4_debug_msgs with tier…
Browse files Browse the repository at this point in the history
…4_internal_debug_msgs

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
  • Loading branch information
mitsudome-r committed Jan 22, 2025
1 parent 93dfb1c commit 5b951a7
Show file tree
Hide file tree
Showing 9 changed files with 18 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/float64.hpp"
#include "tier4_debug_msgs/msg/float64_stamped.hpp"
#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp"

#include <iostream>
#include <memory>
Expand Down Expand Up @@ -71,7 +71,7 @@ class DeviationEstimator : public rclcpp::Node

std::string imu_link_frame_;

std::vector<tier4_debug_msgs::msg::Float64Stamped> vx_all_;
std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> vx_all_;
std::vector<geometry_msgs::msg::Vector3Stamped> gyro_all_;
std::vector<geometry_msgs::msg::PoseStamped> pose_buf_;
std::vector<TrajectoryData> traj_data_list_for_gyro_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "tier4_debug_msgs/msg/float64_stamped.hpp"
#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp"

#include <tf2/transform_datatypes.h>

Expand All @@ -39,7 +39,7 @@
struct TrajectoryData
{
std::vector<geometry_msgs::msg::PoseStamped> pose_list;
std::vector<tier4_debug_msgs::msg::Float64Stamped> vx_list;
std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> vx_list;
std::vector<geometry_msgs::msg::Vector3Stamped> gyro_list;
};

Expand Down Expand Up @@ -95,12 +95,12 @@ double calculate_std_mean_const(const std::vector<T> & v, const double mean)

struct CompareMsgTimestamp
{
bool operator()(tier4_debug_msgs::msg::Float64Stamped const & t1, double const & t2) const
bool operator()(autoware_internal_debug_msgs::msg::Float64Stamped const & t1, double const & t2) const
{
return rclcpp::Time(t1.stamp).seconds() < t2;
}

bool operator()(tier4_debug_msgs::msg::Float64Stamped const & t1, rclcpp::Time const & t2) const
bool operator()(autoware_internal_debug_msgs::msg::Float64Stamped const & t1, rclcpp::Time const & t2) const
{
return rclcpp::Time(t1.stamp).seconds() < t2.seconds();
}
Expand Down Expand Up @@ -163,7 +163,7 @@ double norm_xy(const T p1, const U p2)
double clip_radian(const double rad);

geometry_msgs::msg::Point integrate_position(
const std::vector<tier4_debug_msgs::msg::Float64Stamped> & vx_list,
const std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> & vx_list,
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list, const double coef_vx,
const double yaw_init);

Expand All @@ -176,9 +176,9 @@ geometry_msgs::msg::Vector3 integrate_orientation(
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list,
const geometry_msgs::msg::Vector3 & gyro_bias);

double get_mean_abs_vx(const std::vector<tier4_debug_msgs::msg::Float64Stamped> & vx_list);
double get_mean_abs_vx(const std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> & vx_list);
double get_mean_abs_wz(const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list);
double get_mean_accel(const std::vector<tier4_debug_msgs::msg::Float64Stamped> & vx_list);
double get_mean_accel(const std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> & vx_list);

geometry_msgs::msg::Vector3 transform_vector3(
const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/vector3_stamped.hpp"
#include "tier4_debug_msgs/msg/float64_stamped.hpp"
#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp"

#include <utility>
#include <vector>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>
<depend>autoware_internal_debug_msgs</depend>
<exec_depend>rosbag2_storage_mcap</exec_depend>

<test_depend>ament_cmake_gtest</test_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -202,7 +202,7 @@ void DeviationEstimator::callback_pose_with_covariance(
void DeviationEstimator::callback_wheel_odometry(
const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr)
{
tier4_debug_msgs::msg::Float64Stamped vx;
autoware_internal_debug_msgs::msg::Float64Stamped vx;
vx.stamp = wheel_odometry_msg_ptr->header.stamp;
vx.data = wheel_odometry_msg_ptr->longitudinal_velocity;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ int main(int argc, char ** argv)
if (index >= static_cast<int64_t>(trajectory_data_list.size())) {
trajectory_data_list.resize(index + 1);
}
tier4_debug_msgs::msg::Float64Stamped vx;
autoware_internal_debug_msgs::msg::Float64Stamped vx;
vx.stamp = velocity_status_msg->header.stamp;
vx.data = velocity_status_msg->longitudinal_velocity;
trajectory_data_list[index].vx_list.push_back(vx);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ geometry_msgs::msg::Vector3 interpolate_vector3_stamped(
* point
*/
geometry_msgs::msg::Point integrate_position(
const std::vector<tier4_debug_msgs::msg::Float64Stamped> & vx_list,
const std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> & vx_list,
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list, const double coef_vx,
const double yaw_init)
{
Expand Down Expand Up @@ -153,7 +153,7 @@ geometry_msgs::msg::Vector3 integrate_orientation(
/**
* @brief calculate mean of |vx|
*/
double get_mean_abs_vx(const std::vector<tier4_debug_msgs::msg::Float64Stamped> & vx_list)
double get_mean_abs_vx(const std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> & vx_list)
{
double mean_abs_vx = 0;
for (const auto & msg : vx_list) {
Expand All @@ -179,7 +179,7 @@ double get_mean_abs_wz(const std::vector<geometry_msgs::msg::Vector3Stamped> & g
/**
* @brief calculate mean of acceleration
*/
double get_mean_accel(const std::vector<tier4_debug_msgs::msg::Float64Stamped> & vx_list)
double get_mean_accel(const std::vector<autoware_internal_debug_msgs::msg::Float64Stamped> & vx_list)
{
const double dt =
(rclcpp::Time(vx_list.back().stamp) - rclcpp::Time(vx_list.front().stamp)).seconds();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_srvs/srv/set_bool.hpp"
#include "tier4_debug_msgs/msg/float64_stamped.hpp"
#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp"

#include <tf2/utils.h>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>
<depend>autoware_internal_debug_msgs</depend>

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down

0 comments on commit 5b951a7

Please sign in to comment.