Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] authored and brkay54 committed Jan 31, 2024
1 parent cb628e2 commit ff39bd0
Show file tree
Hide file tree
Showing 4 changed files with 38 additions and 24 deletions.
2 changes: 1 addition & 1 deletion launch/tier4_simulator_launch/launch/simulator.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
</include>
</group>

<!-- Dummy localization -->
<!-- Dummy localization -->
<group if="$(var launch_dummy_localization)">
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_enabled" value="false"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -304,8 +304,8 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
ogm_frame_filtered_pc, *tf2_, base_link_frame_, *base_link_frame_filtered_pc_ptr)) {
return;
}
base_link_frame_filtered_pc_ptr->header.stamp = this->now();
pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr));
base_link_frame_filtered_pc_ptr->header.stamp = this->now();
pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr));
}
if (debugger_ptr_) {
debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header);
Expand Down
55 changes: 35 additions & 20 deletions tools/reaction_analyzer/include/reaction_analyzer_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -159,22 +159,27 @@ template <typename T>
struct PublisherVariablesMessageTypeExtractor;

template <typename MessageType>
struct PublisherVariablesMessageTypeExtractor<PublisherVariables<MessageType>> {
struct PublisherVariablesMessageTypeExtractor<PublisherVariables<MessageType>>
{
using Type = MessageType;
};

template<typename T, typename = std::void_t<>>
struct has_header : std::false_type {};

template<typename T>
struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type {};

template <typename T, typename = std::void_t<>>
struct has_header : std::false_type
{
};

struct PublisherVarAccessor {
template <typename T>
struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
{
};

struct PublisherVarAccessor
{
// Method to set header time if available
template <typename T>
void setHeaderTimeIfAvailable(T& publisherVar, const rclcpp::Time& time) {
void setHeaderTimeIfAvailable(T & publisherVar, const rclcpp::Time & time)
{
if constexpr (has_header<typename T::MessageType>::value) {
if (publisherVar.empty_area_message) {
publisherVar.empty_area_message->header.stamp = time;
Expand All @@ -187,66 +192,76 @@ struct PublisherVarAccessor {

// Set Period
template <typename T>
void setPeriod(T& publisherVar, double newPeriod) {
void setPeriod(T & publisherVar, double newPeriod)
{
publisherVar.period_ns = newPeriod;
}

// Get Period
template <typename T>
double getPeriod(const T& publisherVar) const {
double getPeriod(const T & publisherVar) const
{
return publisherVar.period_ns;
}

// Set Empty Area Message
template <typename T, typename Message>
void setEmptyAreaMessage(T& publisherVar, typename Message::SharedPtr message) {
void setEmptyAreaMessage(T & publisherVar, typename Message::SharedPtr message)
{
publisherVar.empty_area_message = message;
}

// Get Empty Area Message
template <typename T>
std::shared_ptr<void> getEmptyAreaMessage(const T& publisherVar) const {
std::shared_ptr<void> getEmptyAreaMessage(const T & publisherVar) const
{
return std::static_pointer_cast<void>(publisherVar.empty_area_message);
}

// Set Object Spawned Message
template <typename T, typename Message>
void setObjectSpawnedMessage(T& publisherVar, typename Message::SharedPtr message) {
void setObjectSpawnedMessage(T & publisherVar, typename Message::SharedPtr message)
{
publisherVar.object_spawned_message = message;
}

// Get Object Spawned Message
template <typename T>
std::shared_ptr<void> getObjectSpawnedMessage(const T& publisherVar) const {
std::shared_ptr<void> getObjectSpawnedMessage(const T & publisherVar) const
{
return std::static_pointer_cast<void>(publisherVar.object_spawned_message);
}

// Set Publisher
template <typename T, typename PublisherType>
void setPublisher(T& publisherVar, typename rclcpp::Publisher<PublisherType>::SharedPtr publisher) {
void setPublisher(
T & publisherVar, typename rclcpp::Publisher<PublisherType>::SharedPtr publisher)
{
publisherVar.publisher = publisher;
}

// Get Publisher
template <typename T>
std::shared_ptr<void> getPublisher(const T& publisherVar) const {
std::shared_ptr<void> getPublisher(const T & publisherVar) const
{
return std::static_pointer_cast<void>(publisherVar.publisher);
}

// Set Timer
template <typename T>
void setTimer(T& publisherVar, rclcpp::TimerBase::SharedPtr newTimer) {
void setTimer(T & publisherVar, rclcpp::TimerBase::SharedPtr newTimer)
{
publisherVar.timer = newTimer;
}

// Get Timer
template <typename T>
std::shared_ptr<void> getTimer(const T& publisherVar) const {
std::shared_ptr<void> getTimer(const T & publisherVar) const
{
return std::static_pointer_cast<void>(publisherVar.timer);
}
};


using PublisherVariablesVariant = std::variant<
PublisherVariables<PointCloud2>, PublisherVariables<sensor_msgs::msg::CameraInfo>,
PublisherVariables<sensor_msgs::msg::Image>,
Expand Down
1 change: 0 additions & 1 deletion tools/reaction_analyzer/src/reaction_analyzer_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1063,7 +1063,6 @@ void ReactionAnalyzerNode::onTimer()
if (
initialization_state_ptr &&
initialization_state_ptr->state == LocalizationInitializationState::INITIALIZED) {

if (route_state_ptr && route_state_ptr->state != RouteState::SET && !is_route_set_) {
// publish goal pose
is_route_set_ = true;
Expand Down

0 comments on commit ff39bd0

Please sign in to comment.