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!: replace autoware_auto_msgs with autoware_msgs for vehicle modules #7251

Merged
merged 1 commit into from
Jun 4, 2024
Merged
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
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@

#include <Eigen/Dense>

#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "std_msgs/msg/bool.hpp"
Expand Down Expand Up @@ -56,8 +56,8 @@
namespace accel_brake_map_calibrator
{

using autoware_auto_vehicle_msgs::msg::SteeringReport;
using autoware_auto_vehicle_msgs::msg::VelocityReport;
using autoware_vehicle_msgs::msg::SteeringReport;
using autoware_vehicle_msgs::msg::VelocityReport;
using geometry_msgs::msg::TwistStamped;
using nav_msgs::msg::OccupancyGrid;
using raw_vehicle_cmd_converter::AccelMap;
Expand Down
2 changes: 1 addition & 1 deletion vehicle/accel_brake_map_calibrator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>diagnostic_updater</depend>
<depend>geometry_msgs</depend>
<depend>motion_utils</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.

from autoware_auto_vehicle_msgs.msg import GearCommand
from autoware_vehicle_msgs.msg import GearCommand
import rclpy
from rclpy.node import Node
from tier4_debug_msgs.msg import Float32Stamped
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
# limitations under the License.


from autoware_auto_vehicle_msgs.msg import VelocityReport
from autoware_vehicle_msgs.msg import VelocityReport
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy
Expand Down
8 changes: 4 additions & 4 deletions vehicle/external_cmd_converter/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,16 +7,16 @@
| Name | Type | Description |
| --------------------------- | -------------------------------------------- | ----------------------------------------------------------------------------------------------------------------- |
| `~/in/external_control_cmd` | tier4_external_api_msgs::msg::ControlCommand | target `throttle/brake/steering_angle/steering_angle_velocity` is necessary to calculate desired control command. |
| `~/input/shift_cmd"` | autoware_auto_vehicle_msgs::GearCommand | current command of gear. |
| `~/input/shift_cmd"` | autoware_vehicle_msgs::GearCommand | current command of gear. |
| `~/input/emergency_stop` | tier4_external_api_msgs::msg::Heartbeat | emergency heart beat for external command. |
| `~/input/current_gate_mode` | tier4_control_msgs::msg::GateMode | topic for gate mode. |
| `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. |

## Output topics

| Name | Type | Description |
| ------------------- | -------------------------------------------------------- | ------------------------------------------------------------------ |
| `~/out/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | ackermann control command converted from selected external command |
| Name | Type | Description |
| ------------------- | ----------------------------------- | ------------------------------------------------------------------ |
| `~/out/control_cmd` | autoware_control_msgs::msg::Control | ackermann control command converted from selected external command |

## Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@
#include <raw_vehicle_cmd_converter/brake_map.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <autoware_vehicle_msgs/msg/gear_command.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_control_msgs/msg/gate_mode.hpp>
#include <tier4_external_api_msgs/msg/control_command_stamped.hpp>
Expand All @@ -32,13 +32,12 @@

namespace external_cmd_converter
{
using GearCommand = autoware_auto_vehicle_msgs::msg::GearCommand;
using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand;
using GearCommand = autoware_vehicle_msgs::msg::GearCommand;
using Control = autoware_control_msgs::msg::Control;
using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped;
using Odometry = nav_msgs::msg::Odometry;
using raw_vehicle_cmd_converter::AccelMap;
using raw_vehicle_cmd_converter::BrakeMap;
using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand;
using Odometry = nav_msgs::msg::Odometry;

class ExternalCmdConverterNode : public rclcpp::Node
Expand All @@ -48,7 +47,7 @@ class ExternalCmdConverterNode : public rclcpp::Node

private:
// Publisher
rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_cmd_;
rclcpp::Publisher<Control>::SharedPtr pub_cmd_;
rclcpp::Publisher<tier4_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
pub_current_cmd_;

Expand Down
4 changes: 2 additions & 2 deletions vehicle/external_cmd_converter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,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>autoware_vehicle_msgs</depend>
<depend>diagnostic_updater</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
Expand Down
6 changes: 3 additions & 3 deletions vehicle/external_cmd_converter/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n
{
using std::placeholders::_1;

pub_cmd_ = create_publisher<AckermannControlCommand>("out/control_cmd", rclcpp::QoS{1});
pub_cmd_ = create_publisher<Control>("out/control_cmd", rclcpp::QoS{1});
pub_current_cmd_ =
create_publisher<ExternalControlCommand>("out/latest_external_control_cmd", rclcpp::QoS{1});
sub_velocity_ = create_subscription<Odometry>(
Expand Down Expand Up @@ -142,11 +142,11 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const
}

// Publish ControlCommand
autoware_auto_control_msgs::msg::AckermannControlCommand output;
autoware_control_msgs::msg::Control output;
output.stamp = cmd_ptr->stamp;
output.lateral.steering_tire_angle = cmd_ptr->control.steering_angle;
output.lateral.steering_tire_rotation_rate = cmd_ptr->control.steering_angle_velocity;
output.longitudinal.speed = ref_velocity;
output.longitudinal.velocity = ref_velocity;
output.longitudinal.acceleration = ref_acceleration;

pub_cmd_->publish(output);
Expand Down
10 changes: 5 additions & 5 deletions vehicle/raw_vehicle_cmd_converter/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,11 @@ For ease of calibration and adjustments to the lookup table, an auto-calibration

## Input topics

| Name | Type | Description |
| --------------------- | -------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ |
| `~/input/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. |
| `~/input/steering"` | autoware_auto_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control |
| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. |
| Name | Type | Description |
| --------------------- | ------------------------------------- | ------------------------------------------------------------------------------------------------------------------ |
| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. |
| `~/input/steering"` | autoware_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control |
| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. |

## Output topics

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <autoware_vehicle_msgs/msg/steering_report.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_debug_msgs/msg/float32_multi_array_stamped.hpp>
Expand All @@ -36,12 +36,12 @@

namespace raw_vehicle_cmd_converter
{
using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand;
using Control = autoware_control_msgs::msg::Control;
using tier4_debug_msgs::msg::Float32MultiArrayStamped;
using tier4_vehicle_msgs::msg::ActuationCommandStamped;
using TwistStamped = geometry_msgs::msg::TwistStamped;
using Odometry = nav_msgs::msg::Odometry;
using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport;
using Steering = autoware_vehicle_msgs::msg::SteeringReport;

class DebugValues
{
Expand Down Expand Up @@ -77,15 +77,15 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
//!< @brief subscriber for current velocity
rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;
//!< @brief subscriber for vehicle command
rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_control_cmd_;
rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
//!< @brief subscriber for steering
rclcpp::Subscription<Steering>::SharedPtr sub_steering_;

rclcpp::TimerBase::SharedPtr timer_;

std::unique_ptr<TwistStamped> current_twist_ptr_; // [m/s]
std::unique_ptr<double> current_steer_ptr_;
AckermannControlCommand::ConstSharedPtr control_cmd_ptr_;
Control::ConstSharedPtr control_cmd_ptr_;
AccelMap accel_map_;
BrakeMap brake_map_;
SteerMap steer_map_;
Expand All @@ -110,7 +110,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
double calculateBrakeMap(const double current_velocity, const double desired_acc);
double calculateSteer(const double vel, const double steering, const double steer_rate);
void onSteering(const Steering::ConstSharedPtr msg);
void onControlCmd(const AckermannControlCommand::ConstSharedPtr msg);
void onControlCmd(const Control::ConstSharedPtr msg);
void onVelocity(const Odometry::ConstSharedPtr msg);
void publishActuationCmd();
// for debugging
Expand Down
4 changes: 2 additions & 2 deletions vehicle/raw_vehicle_cmd_converter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,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>autoware_vehicle_msgs</depend>
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
<depend>nav_msgs</depend>
Expand Down
4 changes: 2 additions & 2 deletions vehicle/raw_vehicle_cmd_converter/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
steer_pid_.setInitialized();
}
pub_actuation_cmd_ = create_publisher<ActuationCommandStamped>("~/output/actuation_cmd", 1);
sub_control_cmd_ = create_subscription<AckermannControlCommand>(
sub_control_cmd_ = create_subscription<Control>(
"~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1));
sub_velocity_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1));
Expand Down Expand Up @@ -216,7 +216,7 @@ void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr m
current_twist_ptr_->twist = msg->twist.twist;
}

void RawVehicleCommandConverterNode::onControlCmd(const AckermannControlCommand::ConstSharedPtr msg)
void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg)
{
control_cmd_ptr_ = msg;
publishActuationCmd();
Expand Down
8 changes: 4 additions & 4 deletions vehicle/steer_offset_estimator/Readme.md
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,10 @@ Calculate yaw rate error and then calculate steering error recursively by least

### Input

| Name | Type | Description |
| --------------- | ------------------------------------------------- | ------------- |
| `~/input/twist` | `geometry_msgs::msg::TwistStamped` | vehicle twist |
| `~/input/steer` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering |
| Name | Type | Description |
| --------------- | -------------------------------------------- | ------------- |
| `~/input/twist` | `geometry_msgs::msg::TwistStamped` | vehicle twist |
| `~/input/steer` | `autoware_vehicle_msgs::msg::SteeringReport` | steering |

### Output

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include "diagnostic_updater/diagnostic_updater.hpp"
#include "rclcpp/rclcpp.hpp"

#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "tier4_debug_msgs/msg/float32_stamped.hpp"

Expand All @@ -28,7 +28,7 @@ namespace steer_offset_estimator
{
using geometry_msgs::msg::TwistStamped;
using tier4_debug_msgs::msg::Float32Stamped;
using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport;
using Steering = autoware_vehicle_msgs::msg::SteeringReport;
using diagnostic_updater::DiagnosticStatusWrapper;
using diagnostic_updater::Updater;

Expand Down
2 changes: 1 addition & 1 deletion vehicle/steer_offset_estimator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>diagnostic_updater</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
Expand Down
Loading