Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ran precommit and changed config like the one from ros2_control #118

Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 4 additions & 9 deletions .clang-format
Original file line number Diff line number Diff line change
@@ -1,16 +1,11 @@
---
Language: Cpp
BasedOnStyle: Google
Language: Cpp
BasedOnStyle: Google

ColumnLimit: 100
AccessModifierOffset: -2
AlignAfterOpenBracket: AlwaysBreak
BreakBeforeBraces: Custom
BraceWrapping:
AfterClass: true
AfterFunction: true
AfterNamespace: true
AfterStruct: true
ColumnLimit: 100
BreakBeforeBraces: Allman
ConstructorInitializerIndentWidth: 0
ContinuationIndentWidth: 2
DerivePointerAlignment: false
Expand Down
62 changes: 24 additions & 38 deletions .pre-commit-config.yaml
Original file line number Diff line number Diff line change
@@ -1,38 +1,22 @@
# How to use:
#
# Detailed:
# See https://github.com/pre-commit/pre-commit
#
# Short:
# First install pre-commit. For example: pip install pre-commit
#
# You can then install the git hook scripts with:
#
# pre-commit install # (NOTE after this pre-commit runs every time you run git commit)

# To use:
#
# To skip pre-commit you then have to run: git commit -n
# pre-commit run -a
#
# Or:
# Just run pre-commit manually
#
# pre-commit run
#
# To rerun for all files:
#
# pre-commit run -a
# pre-commit install # (runs every time you commit in git)
#
# To update this file:
#
# pre-commit autoupdate
#

# files that should be ignored globally
exclude: ^(docs/_themes/)
# See https://github.com/pre-commit/pre-commit

repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.1.0
rev: v4.4.0
hooks:
- id: check-added-large-files
- id: check-ast
Expand All @@ -46,33 +30,35 @@ repos:
- id: end-of-file-fixer
- id: mixed-line-ending
- id: trailing-whitespace
exclude_types: [rst]
- id: fix-byte-order-marker


# Python hooks
- repo: https://github.com/asottile/pyupgrade
rev: v2.31.1
rev: v3.3.1
hooks:
- id: pyupgrade
args: [--py36-plus]

# PyDocStyle
- repo: https://github.com/PyCQA/pydocstyle
rev: 6.3.0
hooks:
- id: pydocstyle
args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]

- repo: https://github.com/psf/black
rev: 22.3.0
rev: 23.1.0
hooks:
- id: black
args: ["--line-length=99"]

# PEP 257
- repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257
rev: v0.3.3
hooks:
- id: pep257
args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]

- repo: https://github.com/pycqa/flake8
rev: 4.0.1
rev: 6.0.0
hooks:
- id: flake8
args: ["--ignore=E501, W503"]
args: ["--extend-ignore=E501"]

# CPP hooks
- repo: local
Expand Down Expand Up @@ -127,28 +113,28 @@ repos:
stages: [commit]
entry: ament_copyright
language: system
exclude: ^(templates/)|docs/conf.py

# Docs - RestructuredText hooks
- repo: https://github.com/PyCQA/doc8
rev: 0.11.0
rev: v1.1.1
hooks:
- id: doc8
args: ['--max-line-length=100', '--ignore=D001']
exclude: CHANGELOG\.rst$

- repo: https://github.com/pre-commit/pygrep-hooks
rev: v1.9.0
rev: v1.10.0
hooks:
- id: rst-backticks
exclude: CHANGELOG\.rst$
- id: rst-directive-colons
- id: rst-inline-touching-normal

# Spellcheck in comments and docs
# skipping of *.svg files is not working...
- repo: https://github.com/codespell-project/codespell
rev: v2.1.0
rev: v2.2.4
hooks:
- id: codespell
args: ['--write-changes']
exclude: (\.(svg|pyc|drawio))$|(CHANGELOG\.rst)
exclude: CHANGELOG\.rst|\.(svg|pyc)$
96 changes: 65 additions & 31 deletions templates/ros2_control/controller/dummy_chainable_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,9 +61,12 @@ controller_interface::CallbackReturn DummyClassName::on_init()
{
control_mode_.initRT(control_mode_type::FAST);

try {
try
{
param_listener_ = std::make_shared<dummy_controller::ParamListener>(get_node());
} catch (const std::exception & e) {
}
catch (const std::exception & e)
{
fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
}
Expand All @@ -76,13 +79,17 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
{
params_ = param_listener_->get_params();

if (!params_.state_joint_names.empty()) {
if (!params_.state_joint_names.empty())
{
state_joint_names_ = params_.state_joint_names;
} else {
}
else
{
state_joint_names_ = params_.command_joint_names;
}

if (params_.command_joint_names.size() != state_joint_names_.size()) {
if (params_.command_joint_names.size() != state_joint_names_.size())
{
RCLCPP_FATAL(
get_node()->get_logger(),
"Size of 'command_joint_names' (%d) and 'state_joint_names' (%d) parameters has to be the "
Expand All @@ -108,25 +115,32 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
auto set_slow_mode_service_callback =
[&](
const std::shared_ptr<ControllerModeSrvType::Request> request,
std::shared_ptr<ControllerModeSrvType::Response> response) {
if (request->data) {
control_mode_.writeFromNonRT(control_mode_type::SLOW);
} else {
control_mode_.writeFromNonRT(control_mode_type::FAST);
}
response->success = true;
};
std::shared_ptr<ControllerModeSrvType::Response> response)
{
if (request->data)
{
control_mode_.writeFromNonRT(control_mode_type::SLOW);
}
else
{
control_mode_.writeFromNonRT(control_mode_type::FAST);
}
response->success = true;
};

set_slow_control_mode_service_ = get_node()->create_service<ControllerModeSrvType>(
"~/set_slow_control_mode", set_slow_mode_service_callback,
rmw_qos_profile_services_hist_keep_all);

try {
try
{
// State publisher
s_publisher_ = get_node()->create_publisher<ControllerStateMsg>(
"~/controller_state", rclcpp::SystemDefaultsQoS());
state_publisher_ = std::make_unique<ControllerStatePublisher>(s_publisher_);
} catch (const std::exception & e) {
}
catch (const std::exception & e)
{
fprintf(
stderr, "Exception thrown during publisher creation at configure stage with message : %s \n",
e.what());
Expand All @@ -148,7 +162,8 @@ controller_interface::InterfaceConfiguration DummyClassName::command_interface_c
command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

command_interfaces_config.names.reserve(params_.command_joint_names.size());
for (const auto & joint : params_.command_joint_names) {
for (const auto & joint : params_.command_joint_names)
{
command_interfaces_config.names.push_back(joint + "/" + params_.interface_name);
}

Expand All @@ -161,7 +176,8 @@ controller_interface::InterfaceConfiguration DummyClassName::state_interface_con
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

state_interfaces_config.names.reserve(state_joint_names_.size());
for (const auto & joint : state_joint_names_) {
for (const auto & joint : state_joint_names_)
{
state_interfaces_config.names.push_back(joint + "/" + params_.interface_name);
}

Expand All @@ -171,16 +187,21 @@ controller_interface::InterfaceConfiguration DummyClassName::state_interface_con
void DummyClassName::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
{
// if no timestamp provided use current time for command timestamp
if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) {
if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u)
{
RCLCPP_WARN(
get_node()->get_logger(),
"Timestamp in header is missing, using current time as command "
"timestamp.");
msg->header.stamp = get_node()->now();
}
if (msg->joint_names.size() == params_.command_joint_names.size()) {

if (msg->joint_names.size() == params_.command_joint_names.size())
{
input_ref_.writeFromNonRT(msg);
} else {
}
else
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Received %zu , but expected %zu command_joint_names in command. Ignoring message.",
Expand All @@ -195,9 +216,10 @@ std::vector<hardware_interface::CommandInterface> DummyClassName::on_export_refe
std::vector<hardware_interface::CommandInterface> reference_interfaces;
reference_interfaces.reserve(reference_interfaces_.size());

for (size_t i = 0; i < NR_REF_ITFS; ++i) {
for (size_t i = 0; i < NR_REF_ITFS; ++i)
{
reference_interfaces.push_back(hardware_interface::CommandInterface(
get_node()->get_name(), state_joints_[i] + "/" + params_.interface_name,
get_node()->get_name(), state_joint_names_[i] + "/" + params_.interface_name,
&reference_interfaces_[i]));
}

Expand All @@ -224,7 +246,8 @@ controller_interface::CallbackReturn DummyClassName::on_deactivate(
{
// TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
// instead of a loop
for (size_t i = 0; i < NR_CMD_ITFS; ++i) {
for (size_t i = 0; i < NR_CMD_ITFS; ++i)
{
command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
}
return controller_interface::CallbackReturn::SUCCESS;
Expand All @@ -236,8 +259,10 @@ controller_interface::return_type DummyClassName::update_reference_from_subscrib

// TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
// instead of a loop
for (size_t i = 0; i < NR_REF_ITFS; ++i) {
if (!std::isnan((*current_ref)->displacements[i])) {
for (size_t i = 0; i < NR_REF_ITFS; ++i)
{
if (!std::isnan((*current_ref)->displacements[i]))
{
reference_interfaces_[i] = (*current_ref)->displacements[i];

(*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
Expand All @@ -251,25 +276,34 @@ controller_interface::return_type DummyClassName::update_and_write_commands(
{
// TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
// instead of a loop
for (size_t i = 0; i < NR_CMD_ITFS; ++i) {
if (!std::isnan(reference_interfaces_[i])) {
if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
for (size_t i = 0; i < NR_CMD_ITFS; ++i)
{
if (!std::isnan(reference_interfaces_[i]))
{
if (*(control_mode_.readFromRT()) == control_mode_type::SLOW)
{
reference_interfaces_[i] /= 2;
}
command_interfaces_[i].set_value(reference_interfaces_[i]);

reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
}
else
{
command_interfaces_[i].set_value(0.0);
}
}

if (state_publisher_ && state_publisher_->trylock()) {
if (state_publisher_ && state_publisher_->trylock())
{
state_publisher_->msg_.header.stamp = time;
state_publisher_->msg_.set_point = command_interfaces_[NR_CMD_ITFS].get_value();
state_publisher_->msg_.set_point = command_interfaces_[NR_CMD_ITFS - 1].get_value();

state_publisher_->unlockAndPublish();
}

for (size_t i = 0; i < NR_REF_ITFS; ++i) {
for (size_t i = 0; i < NR_REF_ITFS; ++i)
{
reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
}
return controller_interface::return_type::OK;
Expand Down
Loading