Skip to content
Open
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
27 changes: 27 additions & 0 deletions .clang-format
Original file line number Diff line number Diff line change
@@ -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
...
18 changes: 18 additions & 0 deletions .github/workflows/main.yml
Original file line number Diff line number Diff line change
@@ -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 }}

8 changes: 7 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
@@ -1 +1,7 @@
.vscode/
.vscode/
log
build
install
.vscode
.cache
compile_commands.json
17 changes: 9 additions & 8 deletions include/teleop_ack_rc/rc_joy_node.hpp
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
#ifndef RC_JOY_NODE_HPP_
#define RC_JOY_NODE_HPP_

#include <libserial/SerialPort.h> /* SerialPort, Open(), ReadByte(), Read() */

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joy.hpp> /* sensor_msgs::msg::Joy container for axes and button layouts */
#include <libserial/SerialPort.h> /* SerialPort, Open(), ReadByte(), Read() */
#include <sensor_msgs/msg/joy.hpp> /* sensor_msgs::msg::Joy container for axes and button layouts */

namespace teleop_ack_rc {

Expand All @@ -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:
Expand Down
12 changes: 6 additions & 6 deletions include/teleop_ack_rc/teleop_ack_rc_node.hpp
Original file line number Diff line number Diff line change
@@ -1,27 +1,27 @@
#ifndef TELEOP_ACK_RC_NODE_HPP_
#define TELEOP_ACK_RC_NODE_HPP_

#include <ackermann_msgs/msg/ackermann_drive.hpp> /* Provides our ackerman signalling Speed (m/s) and angle (rad)*/
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joy.hpp> /* Normalize our stick values [-1.0, 1.0]*/
#include <ackermann_msgs/msg/ackermann_drive.hpp> /* Provides our ackerman signalling Speed (m/s) and angle (rad)*/
#include <sensor_msgs/msg/joy.hpp> /* 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);

rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
rclcpp::Publisher<ackermann_msgs::msg::AckermannDrive>::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
9 changes: 5 additions & 4 deletions src/rc_joy.cpp
Original file line number Diff line number Diff line change
@@ -1,15 +1,16 @@
#include <memory>
#include <rclcpp/rclcpp.hpp>

#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<teleop_ack_rc::RcJoyNode>(rclcpp::NodeOptions());

rclcpp::spin(node);
rclcpp::shutdown();

return 0;
}
33 changes: 15 additions & 18 deletions src/rc_joy_node.cpp
Original file line number Diff line number Diff line change
@@ -1,42 +1,41 @@
#include "teleop_ack_rc/rc_joy_node.hpp"
#include <vector>
#include <cstring>

#include <algorithm>
#include <cstring>
#include <vector>

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<sensor_msgs::msg::Joy>("/rc_joy", 10);
joy_pub_ = this->create_publisher<sensor_msgs::msg::Joy>("/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. */
Expand All @@ -54,8 +53,7 @@ float RcJoyNode::apply_guardbands(uint16_t raw, int min_v, int max_v, int dead_l
float range = static_cast<float>(max_v) - dead_high;
float out = (val - dead_high) / range;
return std::min(1.0f, out);
}
else {
} else {
float range = static_cast<float>(dead_low) - min_v;
float out = (val - dead_low) / range;
return std::max(-1.0f, out);
Expand Down Expand Up @@ -107,10 +105,9 @@ void RcJoyNode::timer_callback() {
joy_pub_->publish(joy_msg); /* send joy message */
}
} catch (...) {

}
}
}
}

}
} // namespace teleop_ack_rc
9 changes: 5 additions & 4 deletions src/teleop_ack_rc.cpp
Original file line number Diff line number Diff line change
@@ -1,15 +1,16 @@
#include <memory>
#include <rclcpp/rclcpp.hpp>

#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<teleop_ack_rc::TeleopAckRcNode>(rclcpp::NodeOptions());

rclcpp::spin(node);
rclcpp::shutdown();

return 0;
}
15 changes: 7 additions & 8 deletions src/teleop_ack_rc_node.cpp
Original file line number Diff line number Diff line change
@@ -1,20 +1,19 @@
#include "teleop_ack_rc/teleop_ack_rc_node.hpp"

#include <algorithm> /* 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<ackermann_msgs::msg::AckermannDrive>("/ack_vel", 10);

joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
"/rc_joy", 10, std::bind(&TeleopAckRcNode::joy_callback, this, std::placeholders::_1));
}
Expand All @@ -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);
}

}
} // namespace teleop_ack_rc
Loading