Skip to content

Commit

Permalink
Merge pull request #1497 from tier4/refactor/concealer-2
Browse files Browse the repository at this point in the history 
Refactor/concealer 2
  • Loading branch information
@yamacir-kit
yamacir-kit authored Jan 9, 2025
2 parents 12ff08c + c246a77 commit 2f7a478
Show file tree
Hide file tree
Showing 13 changed files with 272 additions and 335 deletions.
34 changes: 15 additions & 19 deletions external/concealer/include/concealer/autoware_universe.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,8 @@
#include <autoware_vehicle_msgs/msg/velocity_report.hpp>
#include <autoware_vehicle_msgs/srv/control_mode_command.hpp>
#include <concealer/continuous_transform_broadcaster.hpp>
#include <concealer/publisher_wrapper.hpp>
#include <concealer/subscriber_wrapper.hpp>
#include <concealer/publisher.hpp>
#include <concealer/subscriber.hpp>
#include <concealer/visibility.hpp>
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
Expand Down Expand Up @@ -57,19 +57,19 @@ class AutowareUniverse : public rclcpp::Node,
using TurnIndicatorsReport = autoware_vehicle_msgs::msg::TurnIndicatorsReport;
using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport;

SubscriberWrapper<Control> getCommand;
SubscriberWrapper<GearCommand> getGearCommand;
SubscriberWrapper<TurnIndicatorsCommand> getTurnIndicatorsCommand;
SubscriberWrapper<PathWithLaneId> getPathWithLaneId;
Subscriber<Control> getCommand;
Subscriber<GearCommand> getGearCommand;
Subscriber<TurnIndicatorsCommand> getTurnIndicatorsCommand;
Subscriber<PathWithLaneId> getPathWithLaneId;

PublisherWrapper<AccelWithCovarianceStamped> setAcceleration;
PublisherWrapper<Odometry> setOdometry;
PublisherWrapper<PoseWithCovarianceStamped> setPose;
PublisherWrapper<SteeringReport> setSteeringReport;
PublisherWrapper<GearReport> setGearReport;
PublisherWrapper<ControlModeReport> setControlModeReport;
PublisherWrapper<VelocityReport> setVelocityReport;
PublisherWrapper<TurnIndicatorsReport> setTurnIndicatorsReport;
Publisher<AccelWithCovarianceStamped> setAcceleration;
Publisher<Odometry> setOdometry;
Publisher<PoseWithCovarianceStamped> setPose;
Publisher<SteeringReport> setSteeringReport;
Publisher<GearReport> setGearReport;
Publisher<ControlModeReport> setControlModeReport;
Publisher<VelocityReport> setVelocityReport;
Publisher<TurnIndicatorsReport> setTurnIndicatorsReport;

std::atomic<geometry_msgs::msg::Accel> current_acceleration;
std::atomic<geometry_msgs::msg::Pose> current_pose;
Expand All @@ -83,7 +83,7 @@ class AutowareUniverse : public rclcpp::Node,

const rclcpp::TimerBase::SharedPtr vehicle_state_update_timer;

std::thread localization_and_vehicle_state_update_thread;
std::thread spinner;

std::atomic<bool> is_stop_requested = false;

Expand All @@ -100,10 +100,6 @@ class AutowareUniverse : public rclcpp::Node,

auto rethrow() -> void;

auto updateLocalization() -> void;

auto updateVehicleState() -> void;

auto getVehicleCommand() const -> std::tuple<double, double, double, double, int>;

auto getRouteLanelets() const -> std::vector<std::int64_t>;
Expand Down
80 changes: 24 additions & 56 deletions external/concealer/include/concealer/field_operator_application.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,9 @@
#include <autoware_vehicle_msgs/msg/gear_command.hpp>
#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
#include <concealer/autoware_universe.hpp>
#include <concealer/launch.hpp>
#include <concealer/publisher_wrapper.hpp>
#include <concealer/service_with_validation.hpp>
#include <concealer/subscriber_wrapper.hpp>
#include <concealer/publisher.hpp>
#include <concealer/service.hpp>
#include <concealer/subscriber.hpp>
#include <concealer/task_queue.hpp>
#include <concealer/transition_assertion.hpp>
#include <concealer/visibility.hpp>
Expand All @@ -55,19 +54,6 @@

namespace concealer
{
/* ---- NOTE -------------------------------------------------------------------
*
* The magic class 'FieldOperatorApplication' is a class that makes it easy to work with
* Autoware from C++. The main features of this class are as follows
*
* (1) Launch Autoware in an independent process upon instantiation of the
* class.
* (2) Properly terminates the Autoware process started by the constructor
* upon destruction of the class.
* (3) Probably the simplest instructions to Autoware, consisting of
* initialize, plan, and engage.
*
* -------------------------------------------------------------------------- */
struct FieldOperatorApplication : public rclcpp::Node,
public TransitionAssertion<FieldOperatorApplication>
{
Expand All @@ -77,11 +63,9 @@ struct FieldOperatorApplication : public rclcpp::Node,

const pid_t process_id = 0;

bool initialize_was_called = false;
bool initialized = false;

std::string autoware_state;

tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array;
std::string autoware_state = "LAUNCHING";

std::string minimum_risk_maneuver_state;

Expand All @@ -107,26 +91,26 @@ struct FieldOperatorApplication : public rclcpp::Node,
using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit;
using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode;

SubscriberWrapper<AutowareState> getAutowareState;
SubscriberWrapper<Control> getCommand;
SubscriberWrapper<CooperateStatusArray> getCooperateStatusArray;
SubscriberWrapper<Emergency> getEmergencyState;
Subscriber<AutowareState> getAutowareState;
Subscriber<Control> getCommand;
Subscriber<CooperateStatusArray> getCooperateStatusArray;
Subscriber<Emergency> getEmergencyState;
#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
SubscriberWrapper<LocalizationInitializationState> getLocalizationState;
Subscriber<LocalizationInitializationState> getLocalizationState;
#endif
SubscriberWrapper<MrmState> getMrmState;
SubscriberWrapper<PathWithLaneId> getPathWithLaneId;
SubscriberWrapper<Trajectory> getTrajectory;
SubscriberWrapper<TurnIndicatorsCommand> getTurnIndicatorsCommand;

ServiceWithValidation<ClearRoute> requestClearRoute;
ServiceWithValidation<CooperateCommands> requestCooperateCommands;
ServiceWithValidation<Engage> requestEngage;
ServiceWithValidation<InitializeLocalization> requestInitialPose;
ServiceWithValidation<SetRoutePoints> requestSetRoutePoints;
ServiceWithValidation<AutoModeWithModule> requestSetRtcAutoMode;
ServiceWithValidation<SetVelocityLimit> requestSetVelocityLimit;
ServiceWithValidation<ChangeOperationMode> requestEnableAutowareControl;
Subscriber<MrmState> getMrmState;
Subscriber<PathWithLaneId> getPathWithLaneId;
Subscriber<Trajectory> getTrajectory;
Subscriber<TurnIndicatorsCommand> getTurnIndicatorsCommand;

Service<ClearRoute> requestClearRoute;
Service<CooperateCommands> requestCooperateCommands;
Service<Engage> requestEngage;
Service<InitializeLocalization> requestInitialPose;
Service<SetRoutePoints> requestSetRoutePoints;
Service<AutoModeWithModule> requestSetRtcAutoMode;
Service<SetVelocityLimit> requestSetVelocityLimit;
Service<ChangeOperationMode> requestEnableAutowareControl;
// clang-format on

/*
Expand All @@ -136,13 +120,7 @@ struct FieldOperatorApplication : public rclcpp::Node,
*/
TaskQueue task_queue;

CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0);

template <typename... Ts>
CONCEALER_PUBLIC explicit FieldOperatorApplication(Ts &&... xs)
: FieldOperatorApplication(ros2_launch(std::forward<decltype(xs)>(xs)...))
{
}
CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t);

~FieldOperatorApplication();

Expand All @@ -160,18 +138,8 @@ struct FieldOperatorApplication : public rclcpp::Node,

auto clearRoute() -> void;

auto getAutowareStateName() const { return autoware_state; }

auto getMinimumRiskManeuverBehaviorName() const { return minimum_risk_maneuver_behavior; }

auto getMinimumRiskManeuverStateName() const { return minimum_risk_maneuver_state; }

auto getEmergencyStateName() const { return minimum_risk_maneuver_state; }

auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray;

auto initialized() const noexcept { return initialize_was_called; }

auto requestAutoModeForCooperation(const std::string &, bool) -> void;

auto rethrow() const { task_queue.rethrow(); }
Expand Down
25 changes: 12 additions & 13 deletions ...r/include/concealer/publisher_wrapper.hpp → ...concealer/include/concealer/publisher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,33 +12,32 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef CONCEALER__PUBLISHER_WRAPPER_HPP_
#define CONCEALER__PUBLISHER_WRAPPER_HPP_
#ifndef CONCEALER__PUBLISHER_HPP_
#define CONCEALER__PUBLISHER_HPP_

#include <memory>
#include <rclcpp/rclcpp.hpp>

namespace concealer
{
template <typename MessageType>
class PublisherWrapper
template <typename Message>
class Publisher
{
private:
typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
typename rclcpp::Publisher<Message>::SharedPtr publisher;

public:
auto operator()(const MessageType & message) const -> decltype(auto)
template <typename Node>
explicit Publisher(const std::string & topic, Node & node)
: publisher(node.template create_publisher<Message>(topic, rclcpp::QoS(1).reliable()))
{
return publisher->publish(message);
}

template <typename NodeInterface>
explicit PublisherWrapper(std::string topic, NodeInterface & autoware_interface)
: publisher(
autoware_interface.template create_publisher<MessageType>(topic, rclcpp::QoS(1).reliable()))
template <typename... Ts>
auto operator()(Ts &&... xs) const -> decltype(auto)
{
return publisher->publish(std::forward<decltype(xs)>(xs)...);
}
};
} // namespace concealer

#endif // CONCEALER__PUBLISHER_WRAPPER_HPP_
#endif // CONCEALER__PUBLISHER_HPP_
18 changes: 9 additions & 9 deletions ...ude/concealer/service_with_validation.hpp → ...l/concealer/include/concealer/service.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef CONCEALER__SERVICE_WITH_VALIDATION_HPP_
#define CONCEALER__SERVICE_WITH_VALIDATION_HPP_
#ifndef CONCEALER__SERVICE_HPP_
#define CONCEALER__SERVICE_HPP_

#include <autoware_adapi_v1_msgs/msg/response_status.hpp>
#include <chrono>
Expand All @@ -28,7 +28,7 @@
namespace concealer
{
template <typename T>
class ServiceWithValidation
class Service
{
const std::string service_name;

Expand All @@ -39,13 +39,13 @@ class ServiceWithValidation
rclcpp::WallRate validation_rate;

public:
template <typename FieldOperatorApplication>
explicit ServiceWithValidation(
const std::string & service_name, FieldOperatorApplication & autoware,
template <typename Node>
explicit Service(
const std::string & service_name, Node & node,
const std::chrono::nanoseconds validation_interval = std::chrono::seconds(1))
: service_name(service_name),
logger(autoware.get_logger()),
client(autoware.template create_client<T>(service_name, rmw_qos_profile_default)),
logger(node.get_logger()),
client(node.template create_client<T>(service_name, rmw_qos_profile_default)),
validation_rate(validation_interval)
{
}
Expand Down Expand Up @@ -142,4 +142,4 @@ class ServiceWithValidation
};
} // namespace concealer

#endif //CONCEALER__SERVICE_WITH_VALIDATION_HPP_
#endif //CONCEALER__SERVICE_HPP_
13 changes: 6 additions & 7 deletions .../include/concealer/subscriber_wrapper.hpp → ...oncealer/include/concealer/subscriber.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,26 +12,25 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef CONCEALER__SUBSCRIBER_WRAPPER_HPP_
#define CONCEALER__SUBSCRIBER_WRAPPER_HPP_
#ifndef CONCEALER__SUBSCRIBER_HPP_
#define CONCEALER__SUBSCRIBER_HPP_

#include <memory>
#include <rclcpp/rclcpp.hpp>

namespace concealer
{
template <typename Message>
class SubscriberWrapper
struct Subscriber
{
typename Message::ConstSharedPtr current_value = std::make_shared<const Message>();

typename rclcpp::Subscription<Message>::SharedPtr subscription;

public:
auto operator()() const -> const auto & { return *std::atomic_load(&current_value); }

template <typename Autoware, typename Callback>
explicit SubscriberWrapper(
explicit Subscriber(
const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware,
const Callback & callback)
: subscription(autoware.template create_subscription<Message>(
Expand All @@ -45,7 +44,7 @@ class SubscriberWrapper
}

template <typename Autoware>
explicit SubscriberWrapper(
explicit Subscriber(
const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware)
: subscription(autoware.template create_subscription<Message>(
topic, quality_of_service, [this](const typename Message::ConstSharedPtr & message) {
Expand All @@ -56,4 +55,4 @@ class SubscriberWrapper
};
} // namespace concealer

#endif // CONCEALER__SUBSCRIBER_WRAPPER_HPP_
#endif // CONCEALER__SUBSCRIBER_HPP_
Loading

0 comments on commit 2f7a478

Please sign in to comment.