Skip to content

Commit

Permalink
delete autoware_auto
Browse files Browse the repository at this point in the history
  • Loading branch information
yabuta committed Jun 7, 2024
1 parent 0ee2399 commit 69360fa
Show file tree
Hide file tree
Showing 18 changed files with 46 additions and 46 deletions.
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
Expand Up @@ -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(
Expand Down Expand Up @@ -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);
}
Expand All @@ -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;
}
Expand Down
4 changes: 2 additions & 2 deletions localization/deviation_estimation_tools/ReadMe.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

```

Expand Down Expand Up @@ -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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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_;
Expand Down Expand Up @@ -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);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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{});
Expand Down Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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:
Expand Down
4 changes: 2 additions & 2 deletions vehicle/calibration_adapter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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_;

Expand All @@ -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
Expand All @@ -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();

Expand Down
2 changes: 1 addition & 1 deletion vehicle/parameter_estimator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
6 changes: 3 additions & 3 deletions vehicle/parameter_estimator/src/parameter_estimator_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));

Expand Down Expand Up @@ -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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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;
Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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;
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion vehicle/time_delay_estimator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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));

Expand Down Expand Up @@ -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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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));

Expand Down Expand Up @@ -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,
Expand Down

0 comments on commit 69360fa

Please sign in to comment.