diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..5668063 --- /dev/null +++ b/.clang-format @@ -0,0 +1,27 @@ +--- +Language: Cpp +BasedOnStyle: Google + +TabWidth: 2 +IndentWidth: 4 +AccessModifierOffset: -4 +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: "false" + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBraces: Custom +ColumnLimit: 120 +DerivePointerAlignment: false +PointerAlignment: Left +ReflowComments: false +... diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 0000000..286d453 --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,18 @@ +name: CI + +on: [ push, pull_request ] + +jobs: + industrial_ci: + runs-on: ubuntu-latest + strategy: + matrix: + env: + - { ROS_DISTRO: humble, ROS_REPO: testing } + - { ROS_DISTRO: humble, ROS_REPO: main } + name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) + steps: + - uses: actions/checkout@v3 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{ matrix.env }} + diff --git a/.gitignore b/.gitignore index dbe9c82..f3a15cc 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,7 @@ -.vscode/ \ No newline at end of file +.vscode/ +log +build +install +.vscode +.cache +compile_commands.json diff --git a/include/teleop_ack_rc/rc_joy_node.hpp b/include/teleop_ack_rc/rc_joy_node.hpp index f2fe57d..e59f9d0 100644 --- a/include/teleop_ack_rc/rc_joy_node.hpp +++ b/include/teleop_ack_rc/rc_joy_node.hpp @@ -1,9 +1,10 @@ #ifndef RC_JOY_NODE_HPP_ #define RC_JOY_NODE_HPP_ +#include /* SerialPort, Open(), ReadByte(), Read() */ + #include -#include /* sensor_msgs::msg::Joy container for axes and button layouts */ -#include /* SerialPort, Open(), ReadByte(), Read() */ +#include /* sensor_msgs::msg::Joy container for axes and button layouts */ namespace teleop_ack_rc { @@ -14,16 +15,16 @@ namespace teleop_ack_rc { * deadman is for PWM-Serial converter internal use only. */ struct __attribute__((packed)) RC_PACKET { - uint8_t header; /* 0xAA */ - uint16_t steer; /* Channel 1 */ - uint16_t throttle; /* Channel 4 */ - uint16_t teleop; /* Channel 5 */ - uint16_t deadman; /* Channel 6 */ + uint8_t header; /* 0xAA */ + uint16_t steer; /* Channel 1 */ + uint16_t throttle; /* Channel 4 */ + uint16_t teleop; /* Channel 5 */ + uint16_t deadman; /* Channel 6 */ }; /* RcJoyNode: responsible for converting PWM values to joy like messages */ class RcJoyNode : public rclcpp::Node { public: - explicit RcJoyNode(const rclcpp::NodeOptions & options); + explicit RcJoyNode(const rclcpp::NodeOptions& options); virtual ~RcJoyNode(); private: diff --git a/include/teleop_ack_rc/teleop_ack_rc_node.hpp b/include/teleop_ack_rc/teleop_ack_rc_node.hpp index e763ff3..250b5d7 100644 --- a/include/teleop_ack_rc/teleop_ack_rc_node.hpp +++ b/include/teleop_ack_rc/teleop_ack_rc_node.hpp @@ -1,16 +1,16 @@ #ifndef TELEOP_ACK_RC_NODE_HPP_ #define TELEOP_ACK_RC_NODE_HPP_ +#include /* Provides our ackerman signalling Speed (m/s) and angle (rad)*/ #include -#include /* Normalize our stick values [-1.0, 1.0]*/ -#include /* Provides our ackerman signalling Speed (m/s) and angle (rad)*/ +#include /* Normalize our stick values [-1.0, 1.0]*/ namespace teleop_ack_rc { /* TeleopAckRcNode: Convert our RC joy messages into ackerman values for drive_mode_switch */ class TeleopAckRcNode : public rclcpp::Node { public: - explicit TeleopAckRcNode(const rclcpp::NodeOptions & options); + explicit TeleopAckRcNode(const rclcpp::NodeOptions& options); private: void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); @@ -18,10 +18,10 @@ class TeleopAckRcNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr ack_pub_; - double max_speed_; /* Maximum speed we will publish [-6.7056, 6.7056]*/ - double max_steering_angle_; /* Maximum angle we will publish [-0.2733, 0.2733]*/ + double max_speed_; /* Maximum speed we will publish [-6.7056, 6.7056]*/ + double max_steering_angle_; /* Maximum angle we will publish [-0.2733, 0.2733]*/ }; -} +} // namespace teleop_ack_rc #endif \ No newline at end of file diff --git a/src/rc_joy.cpp b/src/rc_joy.cpp index 7330e0c..a5ab9de 100644 --- a/src/rc_joy.cpp +++ b/src/rc_joy.cpp @@ -1,15 +1,16 @@ #include #include + #include "teleop_ack_rc/rc_joy_node.hpp" /* main: main entry point for our joy node */ -int main(int argc, char * argv[]) { +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - + auto node = std::make_shared(rclcpp::NodeOptions()); - + rclcpp::spin(node); rclcpp::shutdown(); - + return 0; } \ No newline at end of file diff --git a/src/rc_joy_node.cpp b/src/rc_joy_node.cpp index 15bc847..5503824 100644 --- a/src/rc_joy_node.cpp +++ b/src/rc_joy_node.cpp @@ -1,42 +1,41 @@ #include "teleop_ack_rc/rc_joy_node.hpp" -#include -#include + #include +#include +#include namespace teleop_ack_rc { /* RcJoyNode: Constructor for our Joynode */ -RcJoyNode::RcJoyNode(const rclcpp::NodeOptions & options) - : Node("rc_joy_node", options) { - - this->declare_parameter("port", "/dev/rc_bridge"); /* make parameter to listen to port /dev/rc_bridge as STM will always connect there */ - port_name_ = this->get_parameter("port").as_string(); - +RcJoyNode::RcJoyNode(const rclcpp::NodeOptions& options) : Node("rc_joy_node", options) { + this->declare_parameter( + "port", + "/dev/rc_bridge"); /* make parameter to listen to port /dev/rc_bridge as STM will always connect there */ + port_name_ = this->get_parameter("port").as_string(); + /* Gaurdbands for our steering input */ this->declare_parameter("steer_min", 1038); this->declare_parameter("steer_max", 1990); this->declare_parameter("steer_dead_low", 1493); this->declare_parameter("steer_dead_high", 1504); - + /* Gaurdbands for our steering input */ this->declare_parameter("throttle_min", 1013); this->declare_parameter("throttle_max", 1940); this->declare_parameter("throttle_dead_low", 1492); this->declare_parameter("throttle_dead_high", 1504); - joy_pub_ = this->create_publisher("/rc_joy", 10); + joy_pub_ = this->create_publisher("/rc_joy", 10); try { serial_port_.Open(port_name_); serial_port_.SetBaudRate(LibSerial::BaudRate::BAUD_115200); RCLCPP_INFO(this->get_logger(), "Opened port: %s", port_name_.c_str()); - } catch (const std::exception & e) { + } catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "Serial Error: %s", e.what()); } - timer_ = this->create_wall_timer( - std::chrono::milliseconds(20), - std::bind(&RcJoyNode::timer_callback, this)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&RcJoyNode::timer_callback, this)); } /* RcJoyNode: Destructor closes node on completion. */ @@ -54,8 +53,7 @@ float RcJoyNode::apply_guardbands(uint16_t raw, int min_v, int max_v, int dead_l float range = static_cast(max_v) - dead_high; float out = (val - dead_high) / range; return std::min(1.0f, out); - } - else { + } else { float range = static_cast(dead_low) - min_v; float out = (val - dead_low) / range; return std::max(-1.0f, out); @@ -107,10 +105,9 @@ void RcJoyNode::timer_callback() { joy_pub_->publish(joy_msg); /* send joy message */ } } catch (...) { - } } } } -} \ No newline at end of file +} // namespace teleop_ack_rc \ No newline at end of file diff --git a/src/teleop_ack_rc.cpp b/src/teleop_ack_rc.cpp index 8c8cb38..70b5768 100644 --- a/src/teleop_ack_rc.cpp +++ b/src/teleop_ack_rc.cpp @@ -1,15 +1,16 @@ #include #include + #include "teleop_ack_rc/teleop_ack_rc_node.hpp" /* main: entry point for the teleop_ack_rc node */ -int main(int argc, char * argv[]) { +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - + auto node = std::make_shared(rclcpp::NodeOptions()); - + rclcpp::spin(node); rclcpp::shutdown(); - + return 0; } \ No newline at end of file diff --git a/src/teleop_ack_rc_node.cpp b/src/teleop_ack_rc_node.cpp index b8d7e35..3dc00bc 100644 --- a/src/teleop_ack_rc_node.cpp +++ b/src/teleop_ack_rc_node.cpp @@ -1,20 +1,19 @@ #include "teleop_ack_rc/teleop_ack_rc_node.hpp" + #include /* clamp, min, max */ namespace teleop_ack_rc { /* TeleopAckRcNode: set the parameters for our ackerman and topics */ -TeleopAckRcNode::TeleopAckRcNode(const rclcpp::NodeOptions & options) - : Node("teleop_ack_rc_node", options) { - - this->declare_parameter("max_speed", 6.7056); - this->declare_parameter("max_steering_angle", 0.2733); +TeleopAckRcNode::TeleopAckRcNode(const rclcpp::NodeOptions& options) : Node("teleop_ack_rc_node", options) { + this->declare_parameter("max_speed", 6.7056); + this->declare_parameter("max_steering_angle", 0.2733); max_speed_ = this->get_parameter("max_speed").as_double(); max_steering_angle_ = this->get_parameter("max_steering_angle").as_double(); ack_pub_ = this->create_publisher("/ack_vel", 10); - + joy_sub_ = this->create_subscription( "/rc_joy", 10, std::bind(&TeleopAckRcNode::joy_callback, this, std::placeholders::_1)); } @@ -26,9 +25,9 @@ void TeleopAckRcNode::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) { auto ack_msg = ackermann_msgs::msg::AckermannDrive(); ack_msg.steering_angle = msg->axes[0] * -max_steering_angle_; - ack_msg.speed = msg->axes[1] * max_speed_; + ack_msg.speed = msg->axes[1] * max_speed_; ack_pub_->publish(ack_msg); } -} \ No newline at end of file +} // namespace teleop_ack_rc \ No newline at end of file