Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: convert autoware_auto_msg to autoware_msg #42

Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
delete autoware_auto
yabuta committed Jun 7, 2024
commit 69360fa6ce5929bdfed9e420d9a710ad8f7e72e7
10 changes: 5 additions & 5 deletions control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp
Original file line number Diff line number Diff line change
@@ -76,7 +76,7 @@ void VehicleCmdAnalyzer::publishDebugData()
// set debug values
debug_values_.setValues(DebugValues::TYPE::DT, dt);
debug_values_.setValues(
DebugValues::TYPE::CURRENT_TARGET_VEL, vehicle_cmd_ptr_->longitudinal.speed);
DebugValues::TYPE::CURRENT_TARGET_VEL, vehicle_cmd_ptr_->longitudinal.velocity);
debug_values_.setValues(DebugValues::TYPE::CURRENT_TARGET_D_VEL, d_vel);
debug_values_.setValues(DebugValues::TYPE::CURRENT_TARGET_DD_VEL, dd_vel);
debug_values_.setValues(
@@ -111,13 +111,13 @@ double VehicleCmdAnalyzer::getDt()
std::pair<double, double> VehicleCmdAnalyzer::differentiateVelocity(const double dt)
{
if (!prev_target_vel_) {
prev_target_vel_ = vehicle_cmd_ptr_->longitudinal.speed;
prev_target_vel_ = vehicle_cmd_ptr_->longitudinal.velocity;
prev_target_d_vel_.at(2) = 0.0;
return {0.0, 0.0};
}
const double d_vel = (vehicle_cmd_ptr_->longitudinal.speed - prev_target_vel_) / dt;
const double d_vel = (vehicle_cmd_ptr_->longitudinal.velocity - prev_target_vel_) / dt;
const double dd_vel = (d_vel - prev_target_d_vel_.at(0)) / 2 / dt;
prev_target_vel_ = vehicle_cmd_ptr_->longitudinal.speed;
prev_target_vel_ = vehicle_cmd_ptr_->longitudinal.velocity;
for (int i = 0; i < 2; i++) {
prev_target_d_vel_.at(i) = prev_target_d_vel_.at(i + 1);
}
@@ -139,7 +139,7 @@ double VehicleCmdAnalyzer::differentiateAcceleration(const double dt)
double VehicleCmdAnalyzer::calcLateralAcceleration() const
{
const double delta = vehicle_cmd_ptr_->lateral.steering_tire_angle;
const double vel = vehicle_cmd_ptr_->longitudinal.speed;
const double vel = vehicle_cmd_ptr_->longitudinal.velocity;
const double a_lat = vel * vel * std::sin(delta) / wheelbase_;
return a_lat;
}
4 changes: 2 additions & 2 deletions localization/deviation_estimation_tools/ReadMe.md
Original file line number Diff line number Diff line change
@@ -60,7 +60,7 @@ Topic information: Topic: /localization/pose_estimator/pose_with_covariance | Ty
Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 57309 | Serialization Format: cdr
Topic: /tf_static | Type: tf2_msgs/msg/TFMessage | Count: 2 | Serialization Format: cdr
Topic: /sensing/imu/tamagawa/imu_raw | Type: sensor_msgs/msg/Imu | Count: 8076 | Serialization Format: cdr
Topic: /vehicle/status/velocity_status | Type: autoware_auto_vehicle_msgs/msg/VelocityReport | Count: 8275 | Serialization Format: cdr
Topic: /vehicle/status/velocity_status | Type: autoware_vehicle_msgs/msg/VelocityReport | Count: 8275 | Serialization Format: cdr

```

@@ -187,7 +187,7 @@ The parameters and input topic names can be seen in the `deviation_estimator.lau
| ------------------------ | ------------------------------------------------- | -------------------- |
| `in_pose_with_covariance | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose |
| `in_imu` | `sensor_msgs::msg::Imu` | Input IMU data |
| `in_wheel_odometry` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Input wheel odometry |
| `in_wheel_odometry` | `autoware_vehicle_msgs::msg::VelocityReport` | Input wheel odometry |

#### Output

Original file line number Diff line number Diff line change
@@ -24,7 +24,7 @@
#include "tf2/utils.h"
#include "tier4_autoware_utils/ros/transform_listener.hpp"

#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp"
@@ -58,7 +58,7 @@ class DeviationEstimator : public rclcpp::Node

private:
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr
rclcpp::Subscription<autoware_vehicle_msgs::msg::VelocityReport>::SharedPtr
sub_wheel_odometry_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_coef_vx_;
@@ -109,7 +109,7 @@ class DeviationEstimator : public rclcpp::Node
void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);

void callback_wheel_odometry(
const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr);
const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr);

void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);

Original file line number Diff line number Diff line change
@@ -10,7 +10,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<build_depend>autoware_cmake</build_depend>

<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>rclcpp</depend>
Original file line number Diff line number Diff line change
@@ -167,7 +167,7 @@ DeviationEstimator::DeviationEstimator(

sub_imu_ = create_subscription<sensor_msgs::msg::Imu>(
"in_imu", 1, std::bind(&DeviationEstimator::callback_imu, this, _1));
sub_wheel_odometry_ = create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
sub_wheel_odometry_ = create_subscription<autoware_vehicle_msgs::msg::VelocityReport>(
"in_wheel_odometry", 1, std::bind(&DeviationEstimator::callback_wheel_odometry, this, _1));
results_logger_.log_estimated_result_section(
0.2, 0.0, geometry_msgs::msg::Vector3{}, geometry_msgs::msg::Vector3{});
@@ -200,7 +200,7 @@ void DeviationEstimator::callback_pose_with_covariance(
* @brief receive velocity data and store it in a buffer
*/
void DeviationEstimator::callback_wheel_odometry(
const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr)
const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr)
{
tier4_debug_msgs::msg::Float64Stamped vx;
vx.stamp = wheel_odometry_msg_ptr->header.stamp;
Original file line number Diff line number Diff line change
@@ -58,7 +58,7 @@ int main(int argc, char ** argv)
reader.open(storage_options, converter_options);

// Prepare serialization
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::VelocityReport>
rclcpp::Serialization<autoware_vehicle_msgs::msg::VelocityReport>
serialization_velocity_status;
rclcpp::Serialization<tf2_msgs::msg::TFMessage> serialization_tf;
rclcpp::Serialization<sensor_msgs::msg::Imu> serialization_imu;
@@ -83,8 +83,8 @@ int main(int argc, char ** argv)
const rclcpp::SerializedMessage msg(*serialized_message->serialized_data);

if (topic_name == "/vehicle/status/velocity_status") {
autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr velocity_status_msg =
std::make_shared<autoware_auto_vehicle_msgs::msg::VelocityReport>();
autoware_vehicle_msgs::msg::VelocityReport::SharedPtr velocity_status_msg =
std::make_shared<autoware_vehicle_msgs::msg::VelocityReport>();
serialization_velocity_status.deserialize_message(&msg, velocity_status_msg.get());
const rclcpp::Time curr_stamp = velocity_status_msg->header.stamp;
first_stamp = std::min(first_stamp, curr_stamp);
Original file line number Diff line number Diff line change
@@ -21,17 +21,17 @@
#include "estimator_utils/math_utils.hpp"
#include "rclcpp/rclcpp.hpp"

#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_control_msgs/msg/control.hpp"
#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "tier4_calibration_msgs/msg/float32_stamped.hpp"

#include <vector>

class CalibrationAdapterNode : public CalibrationAdapterNodeBase
{
using Velocity = autoware_auto_vehicle_msgs::msg::VelocityReport;
using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand;
using Velocity = autoware_vehicle_msgs::msg::VelocityReport;
using ControlCommandStamped = autoware_control_msgs::msg::Control;
using TwistStamped = geometry_msgs::msg::TwistStamped;

public:
Original file line number Diff line number Diff line change
@@ -19,8 +19,8 @@

#include "rclcpp/rclcpp.hpp"

#include "autoware_auto_vehicle_msgs/msg/engage.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_vehicle_msgs/msg/engage.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
#include "tier4_calibration_msgs/msg/bool_stamped.hpp"
#include "tier4_calibration_msgs/msg/float32_stamped.hpp"
#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp"
@@ -33,8 +33,8 @@ class CalibrationAdapterNodeBase : public rclcpp::Node
using ActuationStatusStamped = tier4_vehicle_msgs::msg::ActuationStatusStamped;
using Float32Stamped = tier4_calibration_msgs::msg::Float32Stamped;
using BoolStamped = tier4_calibration_msgs::msg::BoolStamped;
using EngageStatus = autoware_auto_vehicle_msgs::msg::Engage;
using SteeringAngleStatus = autoware_auto_vehicle_msgs::msg::SteeringReport;
using EngageStatus = autoware_vehicle_msgs::msg::Engage;
using SteeringAngleStatus = autoware_vehicle_msgs::msg::SteeringReport;
CalibrationAdapterNodeBase();

private:
4 changes: 2 additions & 2 deletions vehicle/calibration_adapter/package.xml
Original file line number Diff line number Diff line change
@@ -11,8 +11,8 @@

<build_depend>autoware_cmake</build_depend>

<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>estimator_utils</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
Original file line number Diff line number Diff line change
@@ -52,7 +52,7 @@ CalibrationAdapterNodeBase::CalibrationAdapterNodeBase() : Node("calibration_ada
pub_is_engage_ =
create_publisher<tier4_calibration_msgs::msg::BoolStamped>("~/output/is_engage", durable_qos);

sub_engage_status_ = create_subscription<autoware_auto_vehicle_msgs::msg::Engage>(
sub_engage_status_ = create_subscription<autoware_vehicle_msgs::msg::Engage>(
"~/input/is_engage", queue_size,
std::bind(&CalibrationAdapterNodeBase::onEngageStatus, this, _1));
sub_actuation_status_ = create_subscription<ActuationStatusStamped>(
Original file line number Diff line number Diff line change
@@ -25,7 +25,7 @@
#include "rclcpp/rclcpp.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"

#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "tier4_calibration_msgs/msg/float32_stamped.hpp"
@@ -38,7 +38,7 @@ class ParameterEstimatorNode : public rclcpp::Node
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_vehicle_twist_;
rclcpp::Subscription<tier4_calibration_msgs::msg::Float32Stamped>::SharedPtr sub_steer_;
rclcpp::Subscription<tier4_calibration_msgs::msg::Float32Stamped>::SharedPtr sub_steer_wheel_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
rclcpp::Subscription<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr
sub_control_mode_report_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;

@@ -50,7 +50,7 @@ class ParameterEstimatorNode : public rclcpp::Node
sensor_msgs::msg::Imu::ConstSharedPtr imu_ptr_;
tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr steer_ptr_;
tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr steer_wheel_ptr_;
autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_ptr_;
autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_ptr_;

/**
* ros parameters
@@ -77,7 +77,7 @@ class ParameterEstimatorNode : public rclcpp::Node
void callbackSteer(const tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr msg);
void callbackSteerWheel(const tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr msg);
void callbackControlModeReport(
const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
void timerCallback();
bool updateGearRatio();

2 changes: 1 addition & 1 deletion vehicle/parameter_estimator/package.xml
Original file line number Diff line number Diff line change
@@ -10,7 +10,7 @@

<buildtool_depend>ament_cmake_auto</buildtool_depend>

<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>estimator_utils</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
6 changes: 3 additions & 3 deletions vehicle/parameter_estimator/src/parameter_estimator_node.cpp
Original file line number Diff line number Diff line change
@@ -74,7 +74,7 @@ ParameterEstimatorNode::ParameterEstimatorNode(const rclcpp::NodeOptions & node_
"input/steer", queue_size, std::bind(&ParameterEstimatorNode::callbackSteer, this, _1));
}
sub_control_mode_report_ =
create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
create_subscription<autoware_vehicle_msgs::msg::ControlModeReport>(
"input/control_mode", queue_size,
std::bind(&ParameterEstimatorNode::callbackControlModeReport, this, _1));

@@ -173,11 +173,11 @@ void ParameterEstimatorNode::callbackSteerWheel(
}

void ParameterEstimatorNode::callbackControlModeReport(
const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg)
const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg)
{
auto & clk = *this->get_clock();
control_mode_ptr_ = msg;
if (control_mode_ptr_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) {
if (control_mode_ptr_->mode == autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) {
auto_mode_duration_ = (this->now().seconds() - last_manual_time_);
RCLCPP_DEBUG_STREAM_THROTTLE(
rclcpp::get_logger("parameter_estimator"), clk, 5000,
Original file line number Diff line number Diff line change
@@ -23,7 +23,7 @@
#include "time_delay_estimator/parameters.hpp"
#include "time_delay_estimator/time_delay_estimator.hpp"

#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
@@ -44,7 +44,7 @@
class TimeDelayEstimatorNode : public rclcpp::Node
{
using Float32Stamped = tier4_calibration_msgs::msg::Float32Stamped;
using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport;
using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport;
using IsEngaged = tier4_calibration_msgs::msg::BoolStamped;
using BoolStamped = tier4_calibration_msgs::msg::BoolStamped;
using TimeDelay = tier4_calibration_msgs::msg::TimeDelay;
@@ -54,7 +54,7 @@ class TimeDelayEstimatorNode : public rclcpp::Node
rclcpp::Publisher<TimeDelay>::SharedPtr pub_time_delay_;

// input subscription
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
rclcpp::Subscription<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr
sub_control_mode_report_;

// response subscription
Original file line number Diff line number Diff line change
@@ -23,7 +23,7 @@
#include "time_delay_estimator/parameters.hpp"
#include "time_delay_estimator/time_delay_estimator.hpp"

#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
@@ -44,7 +44,7 @@
class TimeDelayEstimatorNode : public rclcpp::Node
{
using Float32Stamped = tier4_calibration_msgs::msg::Float32Stamped;
using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport;
using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport;
using IsEngaged = tier4_calibration_msgs::msg::BoolStamped;
using BoolStamped = tier4_calibration_msgs::msg::BoolStamped;
using TimeDelay = tier4_calibration_msgs::msg::TimeDelay;
@@ -57,7 +57,7 @@ class TimeDelayEstimatorNode : public rclcpp::Node
rclcpp::Publisher<TimeDelay>::SharedPtr pub_time_delay_steer_;

// input subscription
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
rclcpp::Subscription<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr
sub_control_mode_;

// response subscription
2 changes: 1 addition & 1 deletion vehicle/time_delay_estimator/package.xml
Original file line number Diff line number Diff line change
@@ -10,7 +10,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>

<!--ros depends-->
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>calibration_adapter</depend>
<depend>eigen</depend>
<depend>estimator_utils</depend>
Original file line number Diff line number Diff line change
@@ -81,7 +81,7 @@ TimeDelayEstimatorNode::TimeDelayEstimatorNode(const rclcpp::NodeOptions & node_
last_manual_time_ = this->now().seconds();
// input
sub_control_mode_report_ =
create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
create_subscription<autoware_vehicle_msgs::msg::ControlModeReport>(
"~/input/control_mode", queue_size,
std::bind(&TimeDelayEstimatorNode::callbackControlModeReport, this, _1));

@@ -189,7 +189,7 @@ void TimeDelayEstimatorNode::callbackControlModeReport(const ControlModeReport::
{
auto & clk = *this->get_clock();
control_mode_ptr_ = msg;
if (control_mode_ptr_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) {
if (control_mode_ptr_->mode == autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) {
auto_mode_duration_ = (this->now().seconds() - last_manual_time_);
RCLCPP_DEBUG_STREAM_THROTTLE(
rclcpp::get_logger("time_delay_estimator"), clk, 5000,
Original file line number Diff line number Diff line change
@@ -86,7 +86,7 @@ TimeDelayEstimatorNode::TimeDelayEstimatorNode(const rclcpp::NodeOptions & node_

last_manual_time_ = this->now().seconds();
// input
sub_control_mode_ = create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
sub_control_mode_ = create_subscription<autoware_vehicle_msgs::msg::ControlModeReport>(
"~/input/control_mode", queue_size,
std::bind(&TimeDelayEstimatorNode::callbackControlModeReport, this, _1));

@@ -296,7 +296,7 @@ void TimeDelayEstimatorNode::callbackControlModeReport(const ControlModeReport::
{
auto & clk = *this->get_clock();
control_mode_ptr_ = msg;
if (control_mode_ptr_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) {
if (control_mode_ptr_->mode == autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) {
auto_mode_duration_ = (this->now().seconds() - last_manual_time_);
RCLCPP_DEBUG_STREAM_THROTTLE(
rclcpp::get_logger("time_delay_estimator"), clk, 5000,