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 tier4_planning_msgs/VelocityLimit to autoware_internal_planning_msgs/VelocityLimit #136

Merged
merged 5 commits into from
Mar 18, 2025
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
1 change: 1 addition & 0 deletions autoware_iv_internal_api_adaptor/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_internal_planning_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_system_msgs</depend>
Expand Down
13 changes: 7 additions & 6 deletions autoware_iv_internal_api_adaptor/src/velocity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,11 +27,11 @@ Velocity::Velocity(const rclcpp::NodeOptions & options) : Node("external_api_vel
srv_velocity_ = proxy.create_service<tier4_external_api_msgs::srv::SetVelocityLimit>(
"/api/autoware/set/velocity_limit", std::bind(&Velocity::setVelocityLimit, this, _1, _2));

pub_api_velocity_ = create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
pub_api_velocity_ = create_publisher<autoware_internal_planning_msgs::msg::VelocityLimit>(
"/api/autoware/get/velocity_limit", rclcpp::QoS(1).transient_local());
pub_planning_velocity_ = create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
pub_planning_velocity_ = create_publisher<autoware_internal_planning_msgs::msg::VelocityLimit>(
"/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1).transient_local());
sub_planning_velocity_ = create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
sub_planning_velocity_ = create_subscription<autoware_internal_planning_msgs::msg::VelocityLimit>(
"/planning/scenario_planning/current_max_velocity", rclcpp::QoS(1).transient_local(),
std::bind(&Velocity::onVelocityLimit, this, _1));

Expand Down Expand Up @@ -63,7 +63,8 @@ void Velocity::setVelocityLimit(
response->status = tier4_api_utils::response_success();
}

void Velocity::onVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::SharedPtr msg)
void Velocity::onVelocityLimit(
const autoware_internal_planning_msgs::msg::VelocityLimit::SharedPtr msg)
{
// store the velocity for releasing the stop
if (kVelocityEpsilon < msg->max_velocity) {
Expand All @@ -75,15 +76,15 @@ void Velocity::onVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::Sh

void Velocity::publishApiVelocity(double velocity)
{
tier4_planning_msgs::msg::VelocityLimit msg;
autoware_internal_planning_msgs::msg::VelocityLimit msg;
msg.stamp = now();
msg.max_velocity = velocity;
pub_api_velocity_->publish(msg);
}

void Velocity::publishPlanningVelocity(double velocity)
{
tier4_planning_msgs::msg::VelocityLimit msg;
autoware_internal_planning_msgs::msg::VelocityLimit msg;
msg.stamp = now();
msg.max_velocity = velocity;
pub_planning_velocity_->publish(msg);
Expand Down
6 changes: 3 additions & 3 deletions autoware_iv_internal_api_adaptor/src/velocity.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,9 @@
#include <rclcpp/rclcpp.hpp>
#include <tier4_api_utils/tier4_api_utils.hpp>

#include <autoware_internal_planning_msgs/msg/velocity_limit.hpp>
#include <tier4_external_api_msgs/srv/pause_driving.hpp>
#include <tier4_external_api_msgs/srv/set_velocity_limit.hpp>
#include <tier4_planning_msgs/msg/velocity_limit.hpp>

namespace internal_api
{
Expand All @@ -32,7 +32,7 @@ class Velocity : public rclcpp::Node
private:
using PauseDriving = tier4_external_api_msgs::srv::PauseDriving;
using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit;
using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit;
using VelocityLimit = autoware_internal_planning_msgs::msg::VelocityLimit;

// ros interface
tier4_api_utils::Service<PauseDriving>::SharedPtr srv_pause_;
Expand All @@ -55,7 +55,7 @@ class Velocity : public rclcpp::Node
void setVelocityLimit(
const tier4_external_api_msgs::srv::SetVelocityLimit::Request::SharedPtr request,
const tier4_external_api_msgs::srv::SetVelocityLimit::Response::SharedPtr response);
void onVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::SharedPtr msg);
void onVelocityLimit(const autoware_internal_planning_msgs::msg::VelocityLimit::SharedPtr msg);

// class method
void publishApiVelocity(double velocity);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,8 @@ class AutowareIvAutowareStatePublisher
const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr & mrm_state_ptr,
tier4_api_msgs::msg::AwapiAutowareStatus * status);
void getCurrentMaxVelInfo(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr,
const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr &
current_max_velocity_ptr,
tier4_api_msgs::msg::AwapiAutowareStatus * status);
void getHazardStatusInfo(
const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <autoware_internal_planning_msgs/msg/velocity_limit.hpp>
#include <autoware_planning_msgs/msg/path.hpp>
#include <autoware_planning_msgs/msg/trajectory.hpp>
#include <autoware_system_msgs/msg/hazard_status_stamped.hpp>
Expand All @@ -37,7 +38,6 @@
#include <tier4_planning_msgs/msg/is_avoidance_possible.hpp>
#include <tier4_planning_msgs/msg/lane_change_status.hpp>
#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
#include <tier4_system_msgs/msg/autoware_state.hpp>
#include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
#include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
Expand Down Expand Up @@ -85,7 +85,7 @@ struct AutowareInfo
tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr;
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr;
tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr max_velocity_ptr;
tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr;
autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr;
tier4_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr;
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr;
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ class AutowareIvAdapter : public rclcpp::Node
rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr
sub_obstacle_avoid_candidate_;
rclcpp::Subscription<tier4_api_msgs::msg::VelocityLimit>::SharedPtr sub_max_velocity_;
rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr
rclcpp::Subscription<autoware_internal_planning_msgs::msg::VelocityLimit>::SharedPtr
sub_current_max_velocity_;
rclcpp::Subscription<tier4_api_msgs::msg::StopCommand>::SharedPtr sub_temporary_stop_;
rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_autoware_traj_;
Expand Down Expand Up @@ -145,7 +145,7 @@ class AutowareIvAdapter : public rclcpp::Node
const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr);
void callbackMaxVelocity(const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr);
void callbackCurrentMaxVelocity(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr);
const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr);
void callbackTemporaryStop(const tier4_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr);
void callbackAutowareTrajectory(
const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <rclcpp/rclcpp.hpp>

#include <tier4_planning_msgs/msg/velocity_limit.hpp>
#include <autoware_internal_planning_msgs/msg/velocity_limit.hpp>

namespace autoware_api
{
Expand All @@ -33,7 +33,7 @@ class AutowareIvMaxVelocityPublisher
rclcpp::Clock::SharedPtr clock_;

// publisher
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_state_;
rclcpp::Publisher<autoware_internal_planning_msgs::msg::VelocityLimit>::SharedPtr pub_state_;

bool calcMaxVelocity(
const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr,
Expand Down
1 change: 1 addition & 0 deletions awapi_awiv_adapter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>autoware_internal_planning_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_system_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
Expand Down
3 changes: 2 additions & 1 deletion awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,8 @@ void AutowareIvAutowareStatePublisher::getEmergencyStateInfo(
}

void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr,
const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr &
current_max_velocity_ptr,
tier4_api_msgs::msg::AwapiAutowareStatus * status)
{
if (!current_max_velocity_ptr) {
Expand Down
9 changes: 5 additions & 4 deletions awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,9 +116,10 @@ AutowareIvAdapter::AutowareIvAdapter()
std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1));
sub_max_velocity_ = this->create_subscription<tier4_api_msgs::msg::VelocityLimit>(
"input/max_velocity", 1, std::bind(&AutowareIvAdapter::callbackMaxVelocity, this, _1));
sub_current_max_velocity_ = this->create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
"input/current_max_velocity", durable_qos,
std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1));
sub_current_max_velocity_ =
this->create_subscription<autoware_internal_planning_msgs::msg::VelocityLimit>(
"input/current_max_velocity", durable_qos,
std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1));
sub_temporary_stop_ = this->create_subscription<tier4_api_msgs::msg::StopCommand>(
"input/temporary_stop", 1, std::bind(&AutowareIvAdapter::callbackTemporaryStop, this, _1));
sub_autoware_traj_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>(
Expand Down Expand Up @@ -323,7 +324,7 @@ void AutowareIvAdapter::callbackMaxVelocity(
}

void AutowareIvAdapter::callbackCurrentMaxVelocity(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr)
const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr)
{
aw_info_.current_max_velocity_ptr = msg_ptr;
}
Expand Down
4 changes: 2 additions & 2 deletions awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,13 @@ AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher(
: clock_(node.get_clock()), default_max_velocity_(default_max_velocity)
{
// publisher
pub_state_ = node.create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
pub_state_ = node.create_publisher<autoware_internal_planning_msgs::msg::VelocityLimit>(
"output/max_velocity", rclcpp::QoS{1}.transient_local());
}

void AutowareIvMaxVelocityPublisher::statePublisher(const AutowareInfo & aw_info)
{
tier4_planning_msgs::msg::VelocityLimit max_velocity;
autoware_internal_planning_msgs::msg::VelocityLimit max_velocity;
if (calcMaxVelocity(
aw_info.max_velocity_ptr, aw_info.temporary_stop_ptr,
&max_velocity.max_velocity)) // publish info
Expand Down
10 changes: 5 additions & 5 deletions build_depends.repos
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
repositories:
core/common:
type: git
url: https://github.com/autowarefoundation/autoware_common.git
version: main
core/autoware.core:
type: git
url: https://github.com/autowarefoundation/autoware.core.git
version: 0.1.0
version: main
core/autoware_utils:
type: git
url: https://github.com/autowarefoundation/autoware_utils.git
version: main
msgs/autoware_msgs:
type: git
url: https://github.com/autowarefoundation/autoware_msgs.git
Expand Down