From 0979ba7ca18e2818dd894ba8c0806f36eb56b442 Mon Sep 17 00:00:00 2001
From: zusizusi <sho.iwasawa.2@tier4.jp>
Date: Wed, 5 Jun 2024 14:25:07 +0900
Subject: [PATCH 1/6] replace subscriber
---
.../raw_vehicle_cmd_converter/src/node.cpp | 25 +++++++++++++++++--
1 file changed, 23 insertions(+), 2 deletions(-)
diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
index 91c668f63dfbd..e95a8238a8542 100644
--- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp
+++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
@@ -75,13 +75,19 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
min_ret_i_steer, max_ret_d_steer, min_ret_d_steer);
steer_pid_.setInitialized();
}
+ rclcpp::CallbackGroup::SharedPtr callback_group =
+ this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+ auto subscription_options = rclcpp::SubscriptionOptions();
+ subscription_options.callback_group = callback_group;
pub_actuation_cmd_ = create_publisher<ActuationCommandStamped>("~/output/actuation_cmd", 1);
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));
+ "~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1),
+ subscription_options);
sub_steering_ = create_subscription<Steering>(
- "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1));
+ "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1),
+ subscription_options);
debug_pub_steer_pid_ = create_publisher<Float32MultiArrayStamped>(
"/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1);
@@ -219,6 +225,21 @@ void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr m
void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg)
{
control_cmd_ptr_ = msg;
+ Odometry::SharedPtr velocity_msg_;
+ Odometry velocity_msg;
+ rclcpp::MessageInfo velocity_info;
+ Steering::SharedPtr steering_msg_;
+ Steering steering_msg;
+ rclcpp::MessageInfo steering_info;
+
+ if (
+ sub_velocity_->take(velocity_msg, velocity_info) &&
+ sub_steering_->take(steering_msg, steering_info)) {
+ velocity_msg_ = std::make_shared<Odometry>(velocity_msg);
+ steering_msg_ = std::make_shared<Steering>(steering_msg);
+ onVelocity(velocity_msg_);
+ onSteering(steering_msg_);
+ }
publishActuationCmd();
}
} // namespace raw_vehicle_cmd_converter
From b9ddf8288cfc4e8c02ed68067c92f1441936b839 Mon Sep 17 00:00:00 2001
From: zusizusi <sho.iwasawa.2@tier4.jp>
Date: Thu, 6 Jun 2024 11:33:25 +0900
Subject: [PATCH 2/6] replace subscriber
---
.../raw_vehicle_cmd_converter/node.hpp | 15 ++++----
.../raw_vehicle_cmd_converter/src/node.cpp | 35 +++++--------------
2 files changed, 17 insertions(+), 33 deletions(-)
diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp
index d71ff96abade0..89dc37ab5d959 100644
--- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp
+++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp
@@ -20,6 +20,7 @@
#include "raw_vehicle_cmd_converter/pid.hpp"
#include "raw_vehicle_cmd_converter/steer_map.hpp"
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
#include <rclcpp/rclcpp.hpp>
@@ -74,12 +75,14 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
//!< @brief topic publisher for low level vehicle command
rclcpp::Publisher<ActuationCommandStamped>::SharedPtr pub_actuation_cmd_;
- //!< @brief subscriber for current velocity
- rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;
//!< @brief subscriber for vehicle command
rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
- //!< @brief subscriber for steering
- rclcpp::Subscription<Steering>::SharedPtr sub_steering_;
+ // polling subscribers
+ tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_velocity_{
+ this, "~/input/odometry"};
+
+ tier4_autoware_utils::InterProcessPollingSubscriber<Steering> sub_steering_{
+ this, "~/input/steering"};
rclcpp::TimerBase::SharedPtr timer_;
@@ -109,9 +112,9 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
const double current_velocity, const double desired_acc, bool & accel_cmd_is_zero);
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 processSteering(const Steering::ConstSharedPtr msg);
void onControlCmd(const Control::ConstSharedPtr msg);
- void onVelocity(const Odometry::ConstSharedPtr msg);
+ void processVelocity(const Odometry::ConstSharedPtr msg);
void publishActuationCmd();
// for debugging
rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_pub_steer_pid_;
diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
index e95a8238a8542..8c3447da436c1 100644
--- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp
+++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
@@ -75,19 +75,9 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
min_ret_i_steer, max_ret_d_steer, min_ret_d_steer);
steer_pid_.setInitialized();
}
- rclcpp::CallbackGroup::SharedPtr callback_group =
- this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
- auto subscription_options = rclcpp::SubscriptionOptions();
- subscription_options.callback_group = callback_group;
pub_actuation_cmd_ = create_publisher<ActuationCommandStamped>("~/output/actuation_cmd", 1);
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),
- subscription_options);
- sub_steering_ = create_subscription<Steering>(
- "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1),
- subscription_options);
debug_pub_steer_pid_ = create_publisher<Float32MultiArrayStamped>(
"/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1);
@@ -210,12 +200,12 @@ double RawVehicleCommandConverterNode::calculateBrakeMap(
return desired_brake_cmd;
}
-void RawVehicleCommandConverterNode::onSteering(const Steering::ConstSharedPtr msg)
+void RawVehicleCommandConverterNode::processSteering(const Steering::ConstSharedPtr msg)
{
current_steer_ptr_ = std::make_unique<double>(msg->steering_tire_angle);
}
-void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr msg)
+void RawVehicleCommandConverterNode::processVelocity(const Odometry::ConstSharedPtr msg)
{
current_twist_ptr_ = std::make_unique<TwistStamped>();
current_twist_ptr_->header = msg->header;
@@ -224,22 +214,13 @@ void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr m
void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg)
{
- control_cmd_ptr_ = msg;
- Odometry::SharedPtr velocity_msg_;
- Odometry velocity_msg;
- rclcpp::MessageInfo velocity_info;
- Steering::SharedPtr steering_msg_;
- Steering steering_msg;
- rclcpp::MessageInfo steering_info;
-
- if (
- sub_velocity_->take(velocity_msg, velocity_info) &&
- sub_steering_->take(steering_msg, steering_info)) {
- velocity_msg_ = std::make_shared<Odometry>(velocity_msg);
- steering_msg_ = std::make_shared<Steering>(steering_msg);
- onVelocity(velocity_msg_);
- onSteering(steering_msg_);
+ const auto velocity_msg = sub_velocity_.takeData();
+ const auto steering_msg = sub_steering_.takeData();
+ if (velocity_msg && steering_msg) {
+ processVelocity(velocity_msg);
+ processSteering(steering_msg);
}
+ control_cmd_ptr_ = msg;
publishActuationCmd();
}
} // namespace raw_vehicle_cmd_converter
From 171981639cb81ede5c352878a6f232140853f71c Mon Sep 17 00:00:00 2001
From: zusizusi <sho.iwasawa.2@tier4.jp>
Date: Thu, 6 Jun 2024 11:48:45 +0900
Subject: [PATCH 3/6] replace subscriber
---
vehicle/raw_vehicle_cmd_converter/src/node.cpp | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
index 8c3447da436c1..6f02298387b2c 100644
--- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp
+++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
@@ -202,11 +202,17 @@ double RawVehicleCommandConverterNode::calculateBrakeMap(
void RawVehicleCommandConverterNode::processSteering(const Steering::ConstSharedPtr msg)
{
+ if (!msg) {
+ return;
+ }
current_steer_ptr_ = std::make_unique<double>(msg->steering_tire_angle);
}
void RawVehicleCommandConverterNode::processVelocity(const Odometry::ConstSharedPtr msg)
{
+ if (!msg) {
+ return;
+ }
current_twist_ptr_ = std::make_unique<TwistStamped>();
current_twist_ptr_->header = msg->header;
current_twist_ptr_->twist = msg->twist.twist;
@@ -216,10 +222,8 @@ void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr
{
const auto velocity_msg = sub_velocity_.takeData();
const auto steering_msg = sub_steering_.takeData();
- if (velocity_msg && steering_msg) {
- processVelocity(velocity_msg);
- processSteering(steering_msg);
- }
+ processVelocity(velocity_msg);
+ processSteering(steering_msg);
control_cmd_ptr_ = msg;
publishActuationCmd();
}
From 5339d5b4b09e33a9d05fd48f8254b83f7a01fe9b Mon Sep 17 00:00:00 2001
From: zusizusi <sho.iwasawa.2@tier4.jp>
Date: Thu, 6 Jun 2024 12:03:10 +0900
Subject: [PATCH 4/6] fix document
---
vehicle/raw_vehicle_cmd_converter/README.md | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md
index 6ac1ee4666cae..ccbce3372f6ae 100644
--- a/vehicle/raw_vehicle_cmd_converter/README.md
+++ b/vehicle/raw_vehicle_cmd_converter/README.md
@@ -29,8 +29,8 @@ For ease of calibration and adjustments to the lookup table, an auto-calibration
| 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. |
+| `~/input/steering"` | autoware_vehicle_msgs::msg::SteeringReport | current status of steering used for steering feed back control |
+| `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. |
## Output topics
From df65d77882183e9a289aa80c199e212d5df894ab Mon Sep 17 00:00:00 2001
From: zusizusi <sho.iwasawa.2@tier4.jp>
Date: Thu, 6 Jun 2024 12:03:47 +0900
Subject: [PATCH 5/6] change function name
---
.../include/raw_vehicle_cmd_converter/node.hpp | 4 ++--
vehicle/raw_vehicle_cmd_converter/src/node.cpp | 6 +++---
2 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp
index 89dc37ab5d959..7096bb5c51a9c 100644
--- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp
+++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp
@@ -78,7 +78,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
//!< @brief subscriber for vehicle command
rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
// polling subscribers
- tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_velocity_{
+ tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_odometry_{
this, "~/input/odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<Steering> sub_steering_{
@@ -114,7 +114,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
double calculateSteer(const double vel, const double steering, const double steer_rate);
void processSteering(const Steering::ConstSharedPtr msg);
void onControlCmd(const Control::ConstSharedPtr msg);
- void processVelocity(const Odometry::ConstSharedPtr msg);
+ void processOdometry(const Odometry::ConstSharedPtr msg);
void publishActuationCmd();
// for debugging
rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_pub_steer_pid_;
diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
index 6f02298387b2c..b274ea6b9806c 100644
--- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp
+++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp
@@ -208,7 +208,7 @@ void RawVehicleCommandConverterNode::processSteering(const Steering::ConstShared
current_steer_ptr_ = std::make_unique<double>(msg->steering_tire_angle);
}
-void RawVehicleCommandConverterNode::processVelocity(const Odometry::ConstSharedPtr msg)
+void RawVehicleCommandConverterNode::processOdometry(const Odometry::ConstSharedPtr msg)
{
if (!msg) {
return;
@@ -220,9 +220,9 @@ void RawVehicleCommandConverterNode::processVelocity(const Odometry::ConstShared
void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg)
{
- const auto velocity_msg = sub_velocity_.takeData();
+ const auto odometry_msg = sub_odometry_.takeData();
const auto steering_msg = sub_steering_.takeData();
- processVelocity(velocity_msg);
+ processOdometry(odometry_msg);
processSteering(steering_msg);
control_cmd_ptr_ = msg;
publishActuationCmd();
From 930240a86ab14933c27701408df6e1abb8e50f5f Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 6 Jun 2024 07:21:59 +0000
Subject: [PATCH 6/6] style(pre-commit): autofix
---
vehicle/raw_vehicle_cmd_converter/README.md | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md
index ccbce3372f6ae..585e5da6c05f6 100644
--- a/vehicle/raw_vehicle_cmd_converter/README.md
+++ b/vehicle/raw_vehicle_cmd_converter/README.md
@@ -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_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. |
+| 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::msg::SteeringReport | current status of steering used for steering feed back control |
-| `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. |
+| `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. |
## Output topics