Skip to content

Commit

Permalink
Misc formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
bsutherland333 committed Nov 3, 2023
1 parent 7131ded commit 4c53317
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 31 deletions.
28 changes: 14 additions & 14 deletions rosplane/include/tuning_signal_generator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -108,41 +108,41 @@ class TuningSignalGenerator : public rclcpp::Node

/// ROS timer to run timer callback, which publishes commands
rclcpp::TimerBase::SharedPtr publish_timer_;

/// ROS parameter change callback handler.
OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;

/// ROS service for toggling step signal
/// ROS service for toggling step signal.
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr step_toggle_service_;
/// ROS service for reset signal
/// ROS service for reset signal.
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
/// ROS service for pause signal
/// ROS service for pause signal.
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pause_service_;
/// ROS service for start signal continuously
/// ROS service for start signal continuously.
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_continuous_service_;
/// ROS service for start signal for one period
/// ROS service for start signal for one period.
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_single_service_;

/// Callback to publish command on topic.
void publish_timer_callback();

/// Callback for parameter changes.
rcl_interfaces::msg::SetParametersResult param_callback(
const std::vector<rclcpp::Parameter> & params);
rcl_interfaces::msg::SetParametersResult
param_callback(const std::vector<rclcpp::Parameter> & params);

/// Callback to toggle step signal
/// Callback to toggle step signal.
bool step_toggle_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
const std_srvs::srv::Trigger::Response::SharedPtr & res);
/// Callback to reset signal
/// Callback to reset signal.
bool reset_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
const std_srvs::srv::Trigger::Response::SharedPtr & res);
/// Callback to pause signal
/// Callback to pause signal.
bool pause_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
const std_srvs::srv::Trigger::Response::SharedPtr & res);
/// Callback to start signal continuously
/// Callback to start signal continuously.
bool start_continuous_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
const std_srvs::srv::Trigger::Response::SharedPtr & res);
/// Callback to start signal for a single period
/// Callback to start signal for a single period.
bool start_single_service_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req,
const std_srvs::srv::Trigger::Response::SharedPtr & res);

Expand Down Expand Up @@ -204,7 +204,7 @@ class TuningSignalGenerator : public rclcpp::Node
/// Updates the parameters within the class with the latest values from ROS.
void update_params();

/// Reset the signal generator
/// Reset the signal generator.
void reset();
};
} // namespace rosplane
Expand Down
30 changes: 13 additions & 17 deletions rosplane/src/tuning_signal_generator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,11 @@ TuningSignalGenerator::TuningSignalGenerator()
update_params();
initial_time_ = this->get_clock()->now().seconds();

internals_publisher_ =
internals_publisher_ =
this->create_publisher<rosplane_msgs::msg::ControllerInternalsDebug>("/tuning_debug", 1);
command_publisher_ =
this->create_publisher<rosplane_msgs::msg::ControllerCommands>("/controller_commands", 1);

publish_timer_ =
this->create_wall_timer(std::chrono::milliseconds(static_cast<long>(1000 / publish_rate_hz_)),
std::bind(&TuningSignalGenerator::publish_timer_callback, this));
Expand Down Expand Up @@ -119,9 +119,7 @@ void TuningSignalGenerator::publish_timer_callback()
}

// Check if step toggle needs to be reset
if (signal_type_ != SignalType::STEP) {
step_toggled_ = false;
}
if (signal_type_ != SignalType::STEP) { step_toggled_ = false; }

// If paused, negate passing of time but keep publishing
if (is_paused_) { paused_time_ += 1 / publish_rate_hz_; }
Expand Down Expand Up @@ -167,7 +165,7 @@ void TuningSignalGenerator::publish_timer_callback()
}

// Creates message with default values
rosplane_msgs::msg::ControllerCommands command_message;
rosplane_msgs::msg::ControllerCommands command_message;
command_message.header.stamp = this->get_clock()->now();
command_message.va_c = default_va_c_;
command_message.h_c = default_h_c_;
Expand Down Expand Up @@ -199,15 +197,12 @@ void TuningSignalGenerator::publish_timer_callback()
internals_publisher_->publish(internals_message);
}

rcl_interfaces::msg::SetParametersResult TuningSignalGenerator::param_callback(
const std::vector<rclcpp::Parameter> & params)
rcl_interfaces::msg::SetParametersResult
TuningSignalGenerator::param_callback(const std::vector<rclcpp::Parameter> & params)
{
for (const auto & param : params)
{
if (param.get_name() == "controller_output" || param.get_name() == "signal_type") {
reset();
}
}
for (const auto & param : params) {
if (param.get_name() == "controller_output" || param.get_name() == "signal_type") { reset(); }
}

rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
Expand Down Expand Up @@ -310,7 +305,7 @@ double TuningSignalGenerator::get_square_signal(double elapsed_time, double ampl
double TuningSignalGenerator::get_sawtooth_signal(double elapsed_time, double amplitude,
double frequency, double center_value)
{
// slope * elapsed_time - num_cycles * offset_per_cycle + center value
// slope * elapsed_time - num_cycles * offset_per_cycle + center value
return 2 * amplitude
* (elapsed_time * frequency - static_cast<int>(elapsed_time * frequency) - 0.5)
+ center_value;
Expand Down Expand Up @@ -394,7 +389,7 @@ void TuningSignalGenerator::update_params()

// default_va_c
default_va_c_ = this->get_parameter("default_va_c").as_double();

// default_h_c
default_h_c_ = this->get_parameter("default_h_c").as_double();

Expand All @@ -408,7 +403,8 @@ void TuningSignalGenerator::update_params()
default_phi_c_ = this->get_parameter("default_phi_c").as_double();
}

void TuningSignalGenerator::reset() {
void TuningSignalGenerator::reset()
{
initial_time_ = this->get_clock()->now().seconds();
paused_time_ = 0;
is_paused_ = true;
Expand Down

0 comments on commit 4c53317

Please sign in to comment.