sync pre-commit and ament uncrustify formatting
atticusrussell opened this issue · 0 comments
atticusrussell commented
pre-commit:
atticus@rospi-ssh-session [publish_ws] <5-sync-pre-commit-and-ament-uncrustify-formatting>
~/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface
check for added large files..............................................Passed
check python ast.....................................(no files to check)Skipped
check for case conflicts.................................................Passed
check docstring is first.............................(no files to check)Skipped
check for merge conflicts................................................Passed
check for broken symlinks................................................Passed
check xml................................................................Passed
check yaml...............................................................Passed
debug statements (python)............................(no files to check)Skipped
detect destroyed symlinks................................................Passed
detect private key.......................................................Passed
fix end of files.........................................................Failed
- hook id: end-of-file-fixer
- exit code: 1
- files were modified by this hook
Fixing include/rpi_pwm_hardware_interface/angular_servo.hpp
forbid submodules....................................(no files to check)Skipped
mixed line ending........................................................Passed
fix requirements.txt.................................(no files to check)Skipped
trim trailing whitespace.................................................Failed
- hook id: trailing-whitespace
- exit code: 1
- files were modified by this hook
Fixing README.md
Fixing src/angular_servo.cpp
fix utf-8 byte order marker..............................................Passed
pyupgrade............................................(no files to check)Skipped
pydocstyle...........................................(no files to check)Skipped
flake8...............................................(no files to check)Skipped
black................................................(no files to check)Skipped
clang-format.............................................................Failed
- hook id: clang-format
- files were modified by this hook
ament_cppcheck...........................................................Passed
ament_cpplint............................................................Failed
- hook id: ament_cpplint
- exit code: 1
Using '--root=/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/include' argument
/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/include/rpi_pwm_hardware_interface/angular_servo.hpp:0: No copyright message found. You should have a line: "Copyright [year] <Copyright Owner>" [legal/copyright] [5]
/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/include/rpi_pwm_hardware_interface/angular_servo.hpp:10: Missing username in TODO; it should look like "// TODO(my_username): Stuff." [readability/todo] [2]
Done processing /home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/include/rpi_pwm_hardware_interface/angular_servo.hpp
Done processing /home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/include/rpi_pwm_hardware_interface/rpi_pwm_hardware_interface.hpp
Done processing /home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/include/rpi_pwm_hardware_interface/visibility_control.h
Using '--root=/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/test' argument
Done processing /home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/test/test_rpi_pwm_hardware_interface.cpp
Category 'legal/copyright' errors found: 1
Category 'readability/todo' errors found: 1
Total errors found: 2
Using '--root=/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/src' argument
/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/src/angular_servo.cpp:0: No copyright message found. You should have a line: "Copyright [year] <Copyright Owner>" [legal/copyright] [5]
/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/src/angular_servo.cpp:8: Do not use namespace using-directives. Use using-declarations instead. [build/namespaces] [5]
/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/src/angular_servo.cpp:10: Missing username in TODO; it should look like "// TODO(my_username): Stuff." [readability/todo] [2]
Done processing /home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/src/angular_servo.cpp
/home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/src/rpi_pwm_hardware_interface.cpp:104: Missing username in TODO; it should look like "// TODO(my_username): Stuff." [readability/todo] [2]
Done processing /home/atticus/workspace/publish_ws/src/ros2_rpi_pwm_hardware_interface/src/rpi_pwm_hardware_interface.cpp
Category 'build/namespaces' errors found: 1
Category 'legal/copyright' errors found: 1
Category 'readability/todo' errors found: 2
Total errors found: 4
ament_lint_cmake.........................................................Passed
ament_copyright..........................................................Failed
- hook id: ament_copyright
- exit code: 1
No problems found, checked 1 files
No problems found, checked 1 files
include/rpi_pwm_hardware_interface/angular_servo.hpp: could not find copyright notice
src/angular_servo.cpp: could not find copyright notice
2 errors, checked 2 files
No problems found, checked 3 files
doc8.................................................(no files to check)Skipped
rst ``code`` is two backticks........................(no files to check)Skipped
rst directives end with two colons...................(no files to check)Skipped
rst ``inline code`` next to normal text..............(no files to check)Skipped
codespell................................................................Passed
pre-commit hook(s) made changes.
If you are seeing this message in CI, reproduce locally with: `pre-commit run --all-files`.
To run `pre-commit` as part of git workflow, use `pre-commit install`.
All changes made by hooks:
diff --git a/README.md b/README.md
index 18c4c59..3bb9a1b 100644
--- a/README.md
+++ b/README.md
@@ -7,7 +7,7 @@ ROS2 control hardware interface package for controlling a servo motor through a
# Dependency on PiGPIO
-This hardware interface requires the pigpio library to interface with the pwm pins on the pi. The installation instructions for pigpio are:
+This hardware interface requires the pigpio library to interface with the pwm pins on the pi. The installation instructions for pigpio are:
wget https://github.com/joan2937/pigpio/archive/master.zip
unzip master.zip
diff --git a/include/rpi_pwm_hardware_interface/angular_servo.hpp b/include/rpi_pwm_hardware_interface/angular_servo.hpp
index e584e52..c4ec773 100644
--- a/include/rpi_pwm_hardware_interface/angular_servo.hpp
+++ b/include/rpi_pwm_hardware_interface/angular_servo.hpp
@@ -1,31 +1,33 @@
#pragma once
-class Servo {
- public:
- Servo(int pi, int pin);
- int getPulseWidth();
- void setPulseWidth(int pulseWidth);
+class Servo
+{
+public:
+ Servo(int pi, int pin);
+ int getPulseWidth();
+ void setPulseWidth(int pulseWidth);
- int __pi; // TODO rename to indicate public, and pigpio pi
- protected:
- int __pin;
+ int __pi; // TODO rename to indicate public, and pigpio pi
+protected:
+ int __pin;
};
-class AngularServo : public Servo {
- public:
- AngularServo(int pi, int pin, float minAngle, float maxAngle, int minPulseWidthUs, int maxPulseWidthUs);
- void setAngle(float angle);
- int getAngle();
+class AngularServo : public Servo
+{
+public:
+ AngularServo(
+ int pi, int pin, float minAngle, float maxAngle, int minPulseWidthUs, int maxPulseWidthUs);
+ void setAngle(float angle);
+ int getAngle();
- protected:
- float __angle;
- float __minAngle;
- float __maxAngle;
- int __minPulseWidthUs;
- int __maxPulseWidthUs;
+protected:
+ float __angle;
+ float __minAngle;
+ float __maxAngle;
+ int __minPulseWidthUs;
+ int __maxPulseWidthUs;
};
bool isPigpiodRunning();
void killPigpiod();
void startPigpiod();
-
diff --git a/include/rpi_pwm_hardware_interface/rpi_pwm_hardware_interface.hpp b/include/rpi_pwm_hardware_interface/rpi_pwm_hardware_interface.hpp
index 6f96eac..3c95ac7 100644
--- a/include/rpi_pwm_hardware_interface/rpi_pwm_hardware_interface.hpp
+++ b/include/rpi_pwm_hardware_interface/rpi_pwm_hardware_interface.hpp
@@ -20,38 +20,37 @@
#include <string>
#include <vector>
-#include "rpi_pwm_hardware_interface/angular_servo.hpp"
-#include "rpi_pwm_hardware_interface/visibility_control.h"
-#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/state.hpp"
+#include "rpi_pwm_hardware_interface/angular_servo.hpp"
+#include "rpi_pwm_hardware_interface/visibility_control.h"
namespace rpi_pwm_hardware_interface
{
class RPiPWMHardwareInterface : public hardware_interface::SystemInterface
{
-
-struct ServoConfig
-{
- std::string name = "";
- int pin = 0;
- int pi = 0;
- float min_angle = 0.0;
- float max_angle = 0.0;
- int min_pulse_width_us = 0;
- int max_pulse_width_us = 0;
-};
-
-struct ServoJoint
-{
- std::string name = "";
- std::unique_ptr<AngularServo> servo;
- double pos = 0;
- double cmd = 0;
-};
+ struct ServoConfig
+ {
+ std::string name = "";
+ int pin = 0;
+ int pi = 0;
+ float min_angle = 0.0;
+ float max_angle = 0.0;
+ int min_pulse_width_us = 0;
+ int max_pulse_width_us = 0;
+ };
+
+ struct ServoJoint
+ {
+ std::string name = "";
+ std::unique_ptr<AngularServo> servo;
+ double pos = 0;
+ double cmd = 0;
+ };
public:
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
diff --git a/src/angular_servo.cpp b/src/angular_servo.cpp
index 0ecbe12..34dbf15 100644
--- a/src/angular_servo.cpp
+++ b/src/angular_servo.cpp
@@ -1,114 +1,149 @@
-#include <iostream>
+#include "rpi_pwm_hardware_interface/angular_servo.hpp"
+#include <pigpiod_if2.h>
#include <csignal>
#include <cstdlib>
#include <functional>
-#include <pigpiod_if2.h>
-#include "rpi_pwm_hardware_interface/angular_servo.hpp"
+#include <iostream>
using namespace std;
-// TODO revisit private vs protected vars - maybe swap to private
+// TODO revisit private vs protected vars - maybe swap to private
-Servo::Servo(int pi, int pin) {
- __pi = pi;
- __pin = pin;
- int rc;
- rc = set_mode(__pi, __pin, PI_OUTPUT);
- if (rc < 0){
- throw "Invalid GPIO pin or mode Error!";
- }
-
- // initializing to zero with pigpio is no input
- rc = set_servo_pulsewidth(__pi, __pin, 0);
- if (rc < 0) {
- throw "Invalid user GPIO pin or pulsewidth Error!";
- }
+Servo::Servo(int pi, int pin)
+{
+ __pi = pi;
+ __pin = pin;
+ int rc;
+ rc = set_mode(__pi, __pin, PI_OUTPUT);
+ if (rc < 0)
+ {
+ throw "Invalid GPIO pin or mode Error!";
+ }
+
+ // initializing to zero with pigpio is no input
+ rc = set_servo_pulsewidth(__pi, __pin, 0);
+ if (rc < 0)
+ {
+ throw "Invalid user GPIO pin or pulsewidth Error!";
+ }
}
-int Servo::getPulseWidth() {
- int rc = get_servo_pulsewidth(__pi, __pin);
- if (rc < 0) {
- throw "Invalid user GPIO pin Error!";
- }
- return rc;
+int Servo::getPulseWidth()
+{
+ int rc = get_servo_pulsewidth(__pi, __pin);
+ if (rc < 0)
+ {
+ throw "Invalid user GPIO pin Error!";
+ }
+ return rc;
}
-void Servo::setPulseWidth(int pulseWidth){
- int rc = set_servo_pulsewidth(__pi, __pin, pulseWidth);
- if (rc < 0) {
- throw "Invalid user GPIO pin or pulsewidth Error!";
- }
+void Servo::setPulseWidth(int pulseWidth)
+{
+ int rc = set_servo_pulsewidth(__pi, __pin, pulseWidth);
+ if (rc < 0)
+ {
+ throw "Invalid user GPIO pin or pulsewidth Error!";
+ }
}
-AngularServo::AngularServo(int pi, int pin, float minAngle, float maxAngle, int minPulseWidthUs, int maxPulseWidthUs) : Servo(pi, pin) {
- __minAngle = minAngle;
- __maxAngle = maxAngle;
- __minPulseWidthUs = minPulseWidthUs;
- __maxPulseWidthUs = maxPulseWidthUs;
+AngularServo::AngularServo(
+ int pi, int pin, float minAngle, float maxAngle, int minPulseWidthUs, int maxPulseWidthUs)
+: Servo(pi, pin)
+{
+ __minAngle = minAngle;
+ __maxAngle = maxAngle;
+ __minPulseWidthUs = minPulseWidthUs;
+ __maxPulseWidthUs = maxPulseWidthUs;
}
-void AngularServo::setAngle(float angle){
- __angle = angle;
- // make sure angle is within bounds
- if (__angle < __minAngle){
- __angle = __minAngle;
- }
- if (__angle > __maxAngle){
- __angle = __maxAngle;
- }
- int pulseWidth = __minPulseWidthUs + (__angle - __minAngle) * (__maxPulseWidthUs - __minPulseWidthUs) / (__maxAngle - __minAngle);
+void AngularServo::setAngle(float angle)
+{
+ __angle = angle;
+ // make sure angle is within bounds
+ if (__angle < __minAngle)
+ {
+ __angle = __minAngle;
+ }
+ if (__angle > __maxAngle)
+ {
+ __angle = __maxAngle;
+ }
+ int pulseWidth = __minPulseWidthUs + (__angle - __minAngle) *
+ (__maxPulseWidthUs - __minPulseWidthUs) /
+ (__maxAngle - __minAngle);
- setPulseWidth(pulseWidth);
+ setPulseWidth(pulseWidth);
}
-int AngularServo::getAngle(){
- int pulseWidth = getPulseWidth();
- float angle = __minAngle + (pulseWidth - __minPulseWidthUs) * (__maxAngle - __minAngle) / (__maxPulseWidthUs - __minPulseWidthUs);
- return angle;
+int AngularServo::getAngle()
+{
+ int pulseWidth = getPulseWidth();
+ float angle = __minAngle + (pulseWidth - __minPulseWidthUs) * (__maxAngle - __minAngle) /
+ (__maxPulseWidthUs - __minPulseWidthUs);
+ return angle;
}
-bool isPigpiodRunning() {
- int result = system("pgrep pigpiod");
+bool isPigpiodRunning()
+{
+ int result = system("pgrep pigpiod");
- if (result == 0) {
- // pigpiod daemon is running
- return true;
- } else {
- // pigpiod daemon is not running
- return false;
- }
+ if (result == 0)
+ {
+ // pigpiod daemon is running
+ return true;
+ }
+ else
+ {
+ // pigpiod daemon is not running
+ return false;
+ }
}
-void killPigpiod() {
- if (isPigpiodRunning()) {
- int result = system("sudo killall pigpiod -q");
-
- if (result == 0) {
- // Successfully killed the pigpiod daemon
- cout << "pigpiod daemon killed successfully" << endl;
- } else {
- // An error occurred while trying to kill the pigpiod daemon
- cerr << "Error killing pigpiod daemon. Return code: " << result << endl;
- }
- } else {
- cout << "pigpiod daemon is not running" << endl;
+void killPigpiod()
+{
+ if (isPigpiodRunning())
+ {
+ int result = system("sudo killall pigpiod -q");
+
+ if (result == 0)
+ {
+ // Successfully killed the pigpiod daemon
+ cout << "pigpiod daemon killed successfully" << endl;
+ }
+ else
+ {
+ // An error occurred while trying to kill the pigpiod daemon
+ cerr << "Error killing pigpiod daemon. Return code: " << result << endl;
}
+ }
+ else
+ {
+ cout << "pigpiod daemon is not running" << endl;
+ }
}
-void startPigpiod() {
- if (!isPigpiodRunning()) {
- cout << "pigpio daemon not running. attempting to start it." << endl;
- system("sudo systemctl start pigpiod");
- sleep(1);
-
- if (isPigpiodRunning()) {
- // Successfully started the pigpiod daemon
- cout << "pigpiod daemon started successfully" << endl;
- } else {
- // An error occurred while trying to start the pigpiod daemon
- cerr << "Error starting pigpiod daemon." << endl;
- }
- } else {
- cout << "pigpiod daemon is already running" << endl;
+void startPigpiod()
+{
+ if (!isPigpiodRunning())
+ {
+ cout << "pigpio daemon not running. attempting to start it." << endl;
+ system("sudo systemctl start pigpiod");
+ sleep(1);
+
+ if (isPigpiodRunning())
+ {
+ // Successfully started the pigpiod daemon
+ cout << "pigpiod daemon started successfully" << endl;
+ }
+ else
+ {
+ // An error occurred while trying to start the pigpiod daemon
+ cerr << "Error starting pigpiod daemon." << endl;
}
+ }
+ else
+ {
+ cout << "pigpiod daemon is already running" << endl;
+ }
}
diff --git a/src/rpi_pwm_hardware_interface.cpp b/src/rpi_pwm_hardware_interface.cpp
index 35af62d..92c574f 100644
--- a/src/rpi_pwm_hardware_interface.cpp
+++ b/src/rpi_pwm_hardware_interface.cpp
@@ -13,14 +13,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <limits>
#include <pigpiod_if2.h>
#include <unistd.h>
+#include <limits>
#include <vector>
-#include "rpi_pwm_hardware_interface/rpi_pwm_hardware_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
+#include "rpi_pwm_hardware_interface/rpi_pwm_hardware_interface.hpp"
namespace rpi_pwm_hardware_interface
{
@@ -47,16 +47,20 @@ hardware_interface::CallbackReturn RPiPWMHardwareInterface::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces
- if (!isPigpiodRunning()) {
- startPigpiod();
- }
-
- if (isPigpiodRunning()) {
- // pigpiod daemon is running
- return CallbackReturn::SUCCESS;
- } else {
- // pigpiod daemon is not running
- return CallbackReturn::ERROR;
+ if (!isPigpiodRunning())
+ {
+ startPigpiod();
+ }
+
+ if (isPigpiodRunning())
+ {
+ // pigpiod daemon is running
+ return CallbackReturn::SUCCESS;
+ }
+ else
+ {
+ // pigpiod daemon is not running
+ return CallbackReturn::ERROR;
}
}
@@ -71,19 +75,20 @@ hardware_interface::CallbackReturn RPiPWMHardwareInterface::on_cleanup(
std::vector<hardware_interface::StateInterface> RPiPWMHardwareInterface::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
- state_interfaces.emplace_back(hardware_interface::StateInterface(
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
// TEST(anyone): insert correct interfaces
rudder_joint_.name, hardware_interface::HW_IF_POSITION, &rudder_joint_.pos));
return state_interfaces;
}
-std::vector<hardware_interface::CommandInterface> RPiPWMHardwareInterface::export_command_interfaces()
+std::vector<hardware_interface::CommandInterface>
+RPiPWMHardwareInterface::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- // TEST(anyone): insert correct interfaces
- rudder_joint_.name, hardware_interface::HW_IF_POSITION, &rudder_joint_.cmd));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ // TEST(anyone): insert correct interfaces
+ rudder_joint_.name, hardware_interface::HW_IF_POSITION, &rudder_joint_.cmd));
return command_interfaces;
}
@@ -94,12 +99,17 @@ hardware_interface::CallbackReturn RPiPWMHardwareInterface::on_activate(
// TEST(anyone): prepare the robot to receive commands
rudder_joint_.name = srv_cfg_.name;
srv_cfg_.pi = pigpio_start(NULL, NULL);
- if (srv_cfg_.pi < 0) {
- // TODO maybe make ros log error
- std::cerr << "Error initializing pigpio" << std::endl;
- return CallbackReturn::ERROR;
- } else {
- rudder_joint_.servo = std::make_unique<AngularServo>(srv_cfg_.pi, srv_cfg_.pin, srv_cfg_.min_angle, srv_cfg_.max_angle, srv_cfg_.min_pulse_width_us, srv_cfg_.max_pulse_width_us);
+ if (srv_cfg_.pi < 0)
+ {
+ // TODO maybe make ros log error
+ std::cerr << "Error initializing pigpio" << std::endl;
+ return CallbackReturn::ERROR;
+ }
+ else
+ {
+ rudder_joint_.servo = std::make_unique<AngularServo>(
+ srv_cfg_.pi, srv_cfg_.pin, srv_cfg_.min_angle, srv_cfg_.max_angle,
+ srv_cfg_.min_pulse_width_us, srv_cfg_.max_pulse_width_us);
return CallbackReturn::SUCCESS;
}
}