Skip to content

Commit

Permalink
chore: replace legacy timer (#329)
Browse files Browse the repository at this point in the history
* chore(goal_distance_calculator): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(path_distance_calculator): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(control_performance_analysis): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(external_cmd_selector): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(joy_controller): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(lane_departure_checker): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(obstacle_collision_checker): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(pure_pursuit): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(shift_decider): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(trajectory_follower_nodes): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(vehicle_cmd_gate): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(ekf_localizer): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(localization_error_monitor): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(multi_object_tracker): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(tensorrt_yolo): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(traffic_light_classifier): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(traffic_light_ssd_fine_detector): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(traffic_light_visualization): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(behavior_path_planner): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(costmap_generator): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(freespace_planner): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(planning_error_monitor): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(scenario_selector): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(pointcloud_preprocessor): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(dummy_perception_publisher): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(ad_service_state_monitor): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(dummy_diag_publisher): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(emergency_handler): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(system_error_monitor): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(topic_state_monitor): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(accel_brake_map_calibrator): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(external_cmd_converter): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(pacmod_interface): replace legacy timer

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* chore(lint): apply pre-commit

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>
  • Loading branch information
wep21 authored Feb 2, 2022
1 parent de23e11 commit c65c057
Show file tree
Hide file tree
Showing 37 changed files with 113 additions and 258 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -56,14 +56,9 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions
self_pose_listener_.waitForFirstPose();

// Timer
double delta_time = 1.0 / static_cast<double>(node_param_.update_rate);
auto timer_callback_ = std::bind(&GoalDistanceCalculatorNode::onTimer, this);
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(delta_time));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback_)>>(
this->get_clock(), period_ns, std::move(timer_callback_),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
const auto period_ns = rclcpp::Rate(node_param_.update_rate).period();
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&GoalDistanceCalculatorNode::onTimer, this));
goal_distance_calculator_ = std::make_unique<GoalDistanceCalculator>();
}

Expand Down
11 changes: 3 additions & 8 deletions common/path_distance_calculator/src/path_distance_calculator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,8 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio
pub_dist_ =
create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/output/distance", rclcpp::QoS(1));

auto timer_callback = [this]() {
using std::chrono_literals::operator""s;
timer_ = rclcpp::create_timer(this, get_clock(), 1s, [this]() {
const auto path = path_;
const auto pose = self_pose_listener_.getCurrentPose();
if (!pose) {
Expand All @@ -53,13 +54,7 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio
msg.stamp = pose->header.stamp;
msg.data = distance;
pub_dist_->publish(msg);
};

using namespace std::literals::chrono_literals;
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), 1s, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
});
}

#include <rclcpp_components/register_node_macro.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,13 +86,10 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode(

// Timer
{
auto on_timer = std::bind(&ControlPerformanceAnalysisNode::onTimer, this);
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(param_.control_period));
timer_publish_ = std::make_shared<rclcpp::GenericTimer<decltype(on_timer)>>(
this->get_clock(), period, std::move(on_timer),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_publish_, nullptr);
timer_publish_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&ControlPerformanceAnalysisNode::onTimer, this));
}

// Wait for first self pose
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -101,12 +101,9 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option
});

// Timer
auto timer_callback = std::bind(&ExternalCmdSelector::onTimer, this);
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / update_rate));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
get_clock(), period, std::move(timer_callback), get_node_base_interface()->get_context());
get_node_timers_interface()->add_timer(timer_, callback_group_subscribers_);
const auto period_ns = rclcpp::Rate(update_rate).period();
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&ExternalCmdSelector::onTimer, this));
}

void ExternalCmdSelector::onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -437,13 +437,10 @@ void AutowareJoyControllerNode::publishVehicleEngage()

void AutowareJoyControllerNode::initTimer(double period_s)
{
auto timer_callback = std::bind(&AutowareJoyControllerNode::onTimer, this);
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&AutowareJoyControllerNode::onTimer, this));
}

AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -173,14 +173,9 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
self_pose_listener_.waitForFirstPose();

// Timer
double delta_time = 1.0 / static_cast<double>(node_param_.update_rate);
auto timer_callback_ = std::bind(&LaneDepartureCheckerNode::onTimer, this);
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(delta_time));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback_)>>(
this->get_clock(), period_ns, std::move(timer_callback_),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
const auto period_ns = rclcpp::Rate(node_param_.update_rate).period();
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this));
}

void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -124,13 +124,10 @@ void ObstacleCollisionCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedP

void ObstacleCollisionCheckerNode::initTimer(double period_s)
{
auto timer_callback = std::bind(&ObstacleCollisionCheckerNode::onTimer, this);
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&ObstacleCollisionCheckerNode::onTimer, this));
}

bool ObstacleCollisionCheckerNode::isDataReady()
Expand Down
9 changes: 3 additions & 6 deletions control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,13 +89,10 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options)

// Timer
{
auto timer_callback = std::bind(&PurePursuitNode::onTimer, this);
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(param_.ctrl_period));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&PurePursuitNode::onTimer, this));
}

// Wait for first current pose
Expand Down
7 changes: 2 additions & 5 deletions control/shift_decider/src/shift_decider.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,13 +69,10 @@ void ShiftDecider::updateCurrentShiftCmd()

void ShiftDecider::initTimer(double period_s)
{
auto timer_callback = std::bind(&ShiftDecider::onTimer, this);
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
timer_ =
rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&ShiftDecider::onTimer, this));
}

#include <rclcpp_components/register_node_macro.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -395,14 +395,10 @@ const

void LateralController::initTimer(float64_t period_s)
{
auto timer_callback = std::bind(&LateralController::onTimer, this);
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<float64_t>(
period_s));
m_timer = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(m_timer, nullptr);
std::chrono::duration<float64_t>(period_s));
m_timer = rclcpp::create_timer(this, get_clock(), period_ns,
std::bind(&LateralController::onTimer, this));
}

void LateralController::declareMPCparameters()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -188,13 +188,9 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_

// Timer
{
auto timer_callback = std::bind(&LongitudinalController::callbackTimerControl, this);
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<float64_t>(1.0 / m_control_rate));
m_timer_control = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(m_timer_control, nullptr);
const auto period_ns = rclcpp::Rate(m_control_rate).period();
m_timer_control = rclcpp::create_timer(this, get_clock(), period_ns,
std::bind(&LongitudinalController::callbackTimerControl, this));
}

// set parameter callback
Expand Down
9 changes: 3 additions & 6 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -189,13 +189,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
start_request_ = std::make_unique<StartRequest>(this, use_start_request);

// Timer
auto timer_callback = std::bind(&VehicleCmdGate::onTimer, this);
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(update_period_));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
timer_ =
rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&VehicleCmdGate::onTimer, this));
}

bool VehicleCmdGate::isHeartbeatTimeout(
Expand Down
21 changes: 7 additions & 14 deletions localization/ekf_localizer/src/ekf_localizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,21 +71,14 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c * ekf_dt_, 2.0);

/* initialize ros system */
auto timer_control_callback = std::bind(&EKFLocalizer::timerCallback, this);
auto period_control =
auto period_control_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(ekf_dt_));
timer_control_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_control_callback)>>(
get_clock(), period_control, std::move(timer_control_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_control_, nullptr);

auto timer_tf_callback = std::bind(&EKFLocalizer::timerTFCallback, this);
auto period_tf = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / tf_rate_));
timer_tf_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_tf_callback)>>(
get_clock(), period_tf, std::move(timer_tf_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_tf_, nullptr);
timer_control_ = rclcpp::create_timer(
this, get_clock(), period_control_ns, std::bind(&EKFLocalizer::timerCallback, this));

const auto period_tf_ns = rclcpp::Rate(tf_rate_).period();
timer_tf_ = rclcpp::create_timer(
this, get_clock(), period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this));

pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
pub_pose_cov_ =
Expand Down
9 changes: 3 additions & 6 deletions localization/ekf_localizer/test/src/test_ekf_localizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,12 +48,9 @@ class TestEKFLocalizerNode : public EKFLocalizer
sub_pose = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"/ekf_pose", 1, std::bind(&TestEKFLocalizerNode::testCallbackPose, this, _1));

auto test_timer_callback = std::bind(&TestEKFLocalizerNode::testTimerCallback, this);
auto period = std::chrono::milliseconds(100);
test_timer_ = std::make_shared<rclcpp::GenericTimer<decltype(test_timer_callback)>>(
this->get_clock(), period, std::move(test_timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(test_timer_, nullptr);
using std::chrono_literals::operator""ms;
test_timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&TestEKFLocalizerNode::testTimerCallback, this));
}
~TestEKFLocalizerNode() {}

Expand Down
10 changes: 3 additions & 7 deletions localization/localization_error_monitor/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,13 +55,9 @@ LocalizationErrorMonitor::LocalizationErrorMonitor()
&LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection);

// Set timer
auto timer_callback = std::bind(&LocalizationErrorMonitor::onTimer, this);
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(0.1));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&LocalizationErrorMonitor::onTimer, this));
}

void LocalizationErrorMonitor::onTimer() { updater_.force_update(); }
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -182,14 +182,9 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)

// Create ROS time based timer
if (enable_delay_compensation) {
auto timer_callback = std::bind(&MultiObjectTracker::onTimer, this);
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / publish_rate));

publish_timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(publish_timer_, nullptr);
const auto period_ns = rclcpp::Rate(publish_rate).period();
publish_timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this));
}

const auto tmp = this->declare_parameter<std::vector<int64_t>>("can_assign_matrix");
Expand Down
11 changes: 3 additions & 8 deletions perception/tensorrt_yolo/src/nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -90,14 +90,9 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options)
new yolo::Net(onnx_file, mode, 1, yolo_config_, calibration_images, calib_cache_file));
net_ptr_->save(engine_file);
}
auto timer_callback = std::bind(&TensorrtYoloNodelet::connectCb, this);
const auto period_s = 0.1;
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&TensorrtYoloNodelet::connectCb, this));

std::lock_guard<std::mutex> lock(connect_mutex_);

Expand Down
12 changes: 3 additions & 9 deletions perception/traffic_light_classifier/src/nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,15 +40,9 @@ TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeO
this->create_publisher<autoware_auto_perception_msgs::msg::TrafficSignalArray>(
"~/output/traffic_signals", rclcpp::QoS{1});

//
auto timer_callback = std::bind(&TrafficLightClassifierNodelet::connectCb, this);
const auto period_s = 0.1;
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&TrafficLightClassifierNodelet::connectCb, this));

int classifier_type = this->declare_parameter(
"classifier_type", static_cast<int>(TrafficLightClassifierNodelet::ClassifierType::HSVFilter));
Expand Down
11 changes: 3 additions & 8 deletions perception/traffic_light_ssd_fine_detector/src/nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -84,14 +84,9 @@ TrafficLightSSDFineDetectorNodelet::TrafficLightSSDFineDetectorNodelet(
mean_ = toFloatVector(this->declare_parameter("mean", std::vector<double>({0.5, 0.5, 0.5})));
std_ = toFloatVector(this->declare_parameter("std", std::vector<double>({0.5, 0.5, 0.5})));

auto timer_callback = std::bind(&TrafficLightSSDFineDetectorNodelet::connectCb, this);
const auto period_s = 0.1;
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&TrafficLightSSDFineDetectorNodelet::connectCb, this));

std::lock_guard<std::mutex> lock(connect_mutex_);
output_roi_pub_ =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,9 @@ TrafficLightRoiVisualizerNodelet::TrafficLightRoiVisualizerNodelet(
std::bind(&TrafficLightRoiVisualizerNodelet::imageRoiCallback, this, _1, _2, _3));
}

auto timer_callback = std::bind(&TrafficLightRoiVisualizerNodelet::connectCb, this);
const auto period_s = 0.1;
const auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
this->get_clock(), period_ns, std::move(timer_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&TrafficLightRoiVisualizerNodelet::connectCb, this));

image_pub_ =
image_transport::create_publisher(this, "~/output/image", rclcpp::QoS{1}.get_rmw_qos_profile());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -131,12 +131,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
// Start timer. This must be done after all data (e.g. vehicle pose, velocity) are ready.
{
const auto planning_hz = declare_parameter("planning_hz", 10.0);
const auto period = rclcpp::Rate(planning_hz).period();
auto on_timer = std::bind(&BehaviorPathPlannerNode::run, this);
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(on_timer)>>(
this->get_clock(), period, std::move(on_timer),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_, nullptr);
const auto period_ns = rclcpp::Rate(planning_hz).period();
timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this));
}
}

Expand Down
Loading

0 comments on commit c65c057

Please sign in to comment.