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

refactor(autoware_auto_control_msgs): replace autoware_auto_control_msgs with autoware_control_msgs #3677

Closed
wants to merge 3 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
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
10 changes: 5 additions & 5 deletions common/tier4_control_rviz_plugin/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,11 @@ This package is to mimic external control for simulation.

### Output

| Name | Type | Description |
| -------------------------------- | ---------------------------------------------------------- | ----------------------- |
| `/control/gate_mode_cmd` | `tier4_control_msgs::msg::GateMode` | GATE mode |
| `/external/selected/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | AckermannControlCommand |
| `/external/selected/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | GEAR |
| Name | Type | Description |
| -------------------------------- | ---------------------------------------------- | ----------- |
| `/control/gate_mode_cmd` | `tier4_control_msgs::msg::GateMode` | GATE mode |
| `/external/selected/control_cmd` | `autoware_control_msgs::msg::Control` | Control |
| `/external/selected/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | GEAR |

## Usage

Expand Down
2 changes: 1 addition & 1 deletion common/tier4_control_rviz_plugin/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>libqt5-core</depend>
<depend>libqt5-gui</depend>
<depend>libqt5-widgets</depend>
Expand Down
12 changes: 6 additions & 6 deletions common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -110,11 +110,11 @@ ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent
void ManualController::update()
{
if (!raw_node_) return;
AckermannControlCommand ackermann;
Control ackermann;
{
ackermann.stamp = raw_node_->get_clock()->now();
ackermann.lateral.steering_tire_angle = steering_angle_;
ackermann.longitudinal.speed = cruise_velocity_;
ackermann.longitudinal.velocity = cruise_velocity_;
if (current_acceleration_) {
/**
* @brief Calculate desired acceleration by simple BackSteppingControl
Expand All @@ -133,9 +133,9 @@ void ManualController::update()
GearCommand gear_cmd;
{
const double eps = 0.001;
if (ackermann.longitudinal.speed > eps) {
if (ackermann.longitudinal.velocity > eps) {
gear_cmd.command = GearCommand::DRIVE;
} else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) {
} else if (ackermann.longitudinal.velocity < -eps && current_velocity_ < eps) {
gear_cmd.command = GearCommand::REVERSE;
ackermann.longitudinal.acceleration *= -1.0;
} else {
Expand Down Expand Up @@ -180,8 +180,8 @@ void ManualController::onInitialize()

pub_gate_mode_ = raw_node_->create_publisher<GateMode>("/control/gate_mode_cmd", rclcpp::QoS(1));

pub_control_command_ = raw_node_->create_publisher<AckermannControlCommand>(
"/external/selected/control_cmd", rclcpp::QoS(1));
pub_control_command_ =
raw_node_->create_publisher<Control>("/external/selected/control_cmd", rclcpp::QoS(1));

pub_gear_cmd_ = raw_node_->create_publisher<GearCommand>("/external/selected/gear_cmd", 1);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,20 +26,20 @@

#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/engage.hpp>
#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <tier4_control_msgs/msg/gate_mode.hpp>
#include <tier4_external_api_msgs/srv/engage.hpp>

#include <memory>

namespace rviz_plugins
{
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_vehicle_msgs::msg::GearCommand;
using autoware_auto_vehicle_msgs::msg::VelocityReport;
using autoware_control_msgs::msg::Control;
using geometry_msgs::msg::Twist;
using tier4_control_msgs::msg::GateMode;
using EngageSrv = tier4_external_api_msgs::srv::Engage;
Expand Down Expand Up @@ -74,7 +74,7 @@ public Q_SLOTS: // NOLINT for Qt
rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_;
rclcpp::Subscription<Engage>::SharedPtr sub_engage_;
rclcpp::Publisher<tier4_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_control_command_;
rclcpp::Publisher<Control>::SharedPtr pub_control_command_;
rclcpp::Publisher<GearCommand>::SharedPtr pub_gear_cmd_;
rclcpp::Client<EngageSrv>::SharedPtr client_engage_;
rclcpp::Subscription<GearReport>::SharedPtr sub_gear_;
Expand Down
2 changes: 1 addition & 1 deletion control/autonomous_emergency_braking/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,9 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_system_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>diagnostic_updater</depend>
<depend>geometry_msgs</depend>
<depend>motion_utils</depend>
Expand Down
14 changes: 7 additions & 7 deletions control/control_performance_analysis/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,13 @@ Error acceleration calculations are made based on the velocity calculations abov

### Input topics

| Name | Type | Description |
| ---------------------------------------- | -------------------------------------------------------- | ------------------------------------------- |
| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. |
| `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | Output control command from control module. |
| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. |
| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. |
| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. |
| Name | Type | Description |
| ---------------------------------------- | ----------------------------------------------- | ------------------------------------------- |
| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. |
| `/control/command/control_cmd` | autoware_control_msgs::msg::Control | Output control command from control module. |
| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. |
| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. |
| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. |

### Output topics

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,9 @@
#include <Eigen/Core>
#include <rclcpp/time.hpp>

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/twist.hpp>
Expand All @@ -38,9 +38,9 @@

namespace control_performance_analysis
{
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
using autoware_control_msgs::msg::Control;
using control_performance_analysis::msg::DrivingMonitorStamped;
using control_performance_analysis::msg::Error;
using control_performance_analysis::msg::ErrorStamped;
Expand Down Expand Up @@ -72,7 +72,7 @@ class ControlPerformanceAnalysisCore
// Setters
void setCurrentPose(const Pose & msg);
void setCurrentWaypoints(const Trajectory & trajectory);
void setCurrentControlValue(const AckermannControlCommand & msg);
void setCurrentControlValue(const Control & msg);
void setInterpolatedVars(
const Pose & interpolated_pose, const double & interpolated_velocity,
const double & interpolated_acceleration, const double & interpolated_steering_angle);
Expand Down Expand Up @@ -102,7 +102,7 @@ class ControlPerformanceAnalysisCore
std::shared_ptr<autoware_auto_planning_msgs::msg::Trajectory> current_trajectory_ptr_;
std::shared_ptr<Pose> current_vec_pose_ptr_;
std::shared_ptr<std::vector<Odometry>> odom_history_ptr_; // velocities at k-2, k-1, k, k+1
std::shared_ptr<AckermannControlCommand> current_control_ptr_;
std::shared_ptr<Control> current_control_ptr_;
std::shared_ptr<SteeringReport> current_vec_steering_msg_ptr_;

// State holder
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,9 @@
#include <signal_processing/lowpass_filter_1d.hpp>
#include <tier4_autoware_utils/ros/self_pose_listener.hpp>

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>

Expand All @@ -36,9 +36,9 @@

namespace control_performance_analysis
{
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
using autoware_control_msgs::msg::Control;
using control_performance_analysis::msg::DrivingMonitorStamped;
using control_performance_analysis::msg::ErrorStamped;
using geometry_msgs::msg::PoseStamped;
Expand All @@ -52,9 +52,8 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node
private:
// Subscribers and Local Variable Assignment
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_; // subscribe to trajectory
rclcpp::Subscription<AckermannControlCommand>::SharedPtr
sub_control_cmd_; // subscribe to steering control value
rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_; // subscribe to velocity
rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_; // subscribe to steering control value
rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_; // subscribe to velocity
rclcpp::Subscription<SteeringReport>::SharedPtr sub_vehicle_steering_;

// Publishers
Expand All @@ -68,26 +67,26 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node

// Callback Methods
void onTrajectory(const Trajectory::ConstSharedPtr msg);
void onControlRaw(const AckermannControlCommand::ConstSharedPtr control_msg);
void onControlRaw(const Control::ConstSharedPtr control_msg);
void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg);
void onVelocity(const Odometry::ConstSharedPtr msg);

// Parameters
Params param_{}; // wheelbase, control period and feedback coefficients.
// State holder
AckermannControlCommand::ConstSharedPtr last_control_cmd_;
Control::ConstSharedPtr last_control_cmd_;
double d_control_cmd_{0};

// Subscriber Parameters
Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj.
AckermannControlCommand::ConstSharedPtr current_control_msg_ptr_;
Control::ConstSharedPtr current_control_msg_ptr_;
SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_;
Odometry::ConstSharedPtr current_odom_ptr_;
PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading

// prev states
Trajectory prev_traj;
AckermannControlCommand prev_cmd;
Control prev_cmd;
SteeringReport prev_steering;

// Algorithm
Expand Down
2 changes: 1 addition & 1 deletion control/control_performance_analysis/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,9 @@

<build_depend>builtin_interfaces</build_depend>

<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>geometry_msgs</depend>
<depend>libboost-dev</depend>
<depend>motion_utils</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -74,9 +74,9 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg)
current_vec_pose_ptr_ = std::make_shared<Pose>(msg);
}

void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannControlCommand & msg)
void ControlPerformanceAnalysisCore::setCurrentControlValue(const Control & msg)
{
current_control_ptr_ = std::make_shared<AckermannControlCommand>(msg);
current_control_ptr_ = std::make_shared<Control>(msg);
}

std::pair<bool, int32_t> ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@

namespace
{
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_control_msgs::msg::Control;
using control_performance_analysis::msg::DrivingMonitorStamped;
using control_performance_analysis::msg::ErrorStamped;

Expand Down Expand Up @@ -62,7 +62,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode(
"~/input/reference_trajectory", 1,
std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1));

sub_control_cmd_ = create_subscription<AckermannControlCommand>(
sub_control_cmd_ = create_subscription<Control>(
"~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1));

sub_vehicle_steering_ = create_subscription<SteeringReport>(
Expand Down Expand Up @@ -93,8 +93,7 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP
current_trajectory_ptr_ = msg;
}

void ControlPerformanceAnalysisNode::onControlRaw(
const AckermannControlCommand::ConstSharedPtr control_msg)
void ControlPerformanceAnalysisNode::onControlRaw(const Control::ConstSharedPtr control_msg)
{
if (last_control_cmd_) {
const rclcpp::Duration & duration =
Expand Down
18 changes: 9 additions & 9 deletions control/joy_controller/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,15 @@

### Output topics

| Name | Type | Description |
| ----------------------------------- | -------------------------------------------------------- | ---------------------------------------- |
| `~/output/control_command` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command |
| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command |
| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command |
| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command |
| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) |
| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat |
| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage |
| Name | Type | Description |
| ----------------------------------- | --------------------------------------------------- | ---------------------------------------- |
| `~/output/control_command` | autoware_control_msgs::msg::Control | lateral and longitudinal control command |
| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command |
| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command |
| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command |
| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) |
| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat |
| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage |

## Parameters

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_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/engage.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/joy.hpp>
Expand Down Expand Up @@ -80,8 +80,7 @@ class AutowareJoyControllerNode : public rclcpp::Node
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);

// Publisher
rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
pub_control_command_;
rclcpp::Publisher<autoware_control_msgs::msg::Control>::SharedPtr pub_control_command_;
rclcpp::Publisher<tier4_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
pub_external_control_command_;
rclcpp::Publisher<tier4_external_api_msgs::msg::GearShiftStamped>::SharedPtr pub_shift_;
Expand All @@ -105,7 +104,7 @@ class AutowareJoyControllerNode : public rclcpp::Node
rclcpp::Client<tier4_external_api_msgs::srv::Engage>::SharedPtr client_autoware_engage_;

// Previous State
autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_command_;
autoware_control_msgs::msg::Control prev_control_command_;
tier4_external_api_msgs::msg::ControlCommand prev_external_control_command_;
GearShiftType prev_shift_ = tier4_external_api_msgs::msg::GearShift::NONE;
TurnSignalType prev_turn_signal_ = tier4_external_api_msgs::msg::TurnSignal::NONE;
Expand Down
2 changes: 1 addition & 1 deletion control/joy_controller/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>geometry_msgs</depend>
<depend>joy</depend>
<depend>nav_msgs</depend>
Expand Down
Loading