Skip to content

Commit

Permalink
Reformatting and chenged comments.
Browse files Browse the repository at this point in the history
  • Loading branch information
@alekskl01
alekskl01 committed Mar 19, 2024
1 parent 9988883 commit b3f4d9b
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 32 deletions.
1 change: 0 additions & 1 deletion ...ruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
#include <sys/ioctl.h>
#include <unistd.h> // Used for the close function

// Declare functions we can use
namespace thruster_interface_asv_driver_lib {
void init(const std::string &pathToCSVFile, int8_t *thrusterMapping,
int8_t *thrusterDirection, int16_t *offsetPWM, int16_t *minPWM,
Expand Down
34 changes: 15 additions & 19 deletions motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,9 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) {
std::vector<_ForcePWM> data;

// Try opening the file and if suceeded fill our datastructure with all the
// values in the .CSV file
// values from the force mapping .CSV file
std::ifstream file(filepath);
if (file.is_open()) {
// Variable for the currect line value in the .CSV file
std::string line;

// Skip the header line
Expand All @@ -29,20 +28,24 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) {
// Temporarty variables to store data correctly ----------
// Define temporary placeholders for variables we are extracting
std::string tempVar;

// Define data structure format we want our .CSV vlaues to be ordered in
_ForcePWM ForcePWMDataStructure;

// Data manipulation ----------
// csvLineSplit variable converts "line" variable to a char stream of data
// that can further be used to split and filter out values we want
std::istringstream csvLineSplit(line);

// Extract Forces from "csvLineSplit" variable
std::getline(csvLineSplit, tempVar, '\t');
ForcePWMDataStructure.force = std::stof(tempVar);

// Convert grams into Newtons as we expect to get Forces in Newtons but
// the .CSV file calculates forsces in grams
// the .CSV file contains forces in grams
ForcePWMDataStructure.force =
ForcePWMDataStructure.force * (9.81 / 1000.0);

// Extract PWM from "csvLineSplit" variable
std::getline(csvLineSplit, tempVar, '\t');
ForcePWMDataStructure.pwm = std::stof(tempVar);
Expand Down Expand Up @@ -70,10 +73,12 @@ int16_t *_interpolate_force_to_pwm(float *forces) {
// accordingly
if (forces[i] <= _ForcePWMTable.front().force) {
interpolatedPWMArray[i] =
static_cast<int16_t>(_ForcePWMTable.front().pwm); // To small Force
static_cast<int16_t>(_ForcePWMTable.front().pwm); // Too small Force

} else if (forces[i] >= _ForcePWMTable.back().force) {
interpolatedPWMArray[i] =
static_cast<int16_t>(_ForcePWMTable.back().pwm); // To big Force
static_cast<int16_t>(_ForcePWMTable.back().pwm); // Too big Force

} else {
// Set temporary variables for interpolating
// Initialize with the first element
Expand Down Expand Up @@ -168,16 +173,9 @@ void init(const std::string &pathToCSVFile, int8_t *thrusterMapping,
int8_t *thrusterDirection, int16_t *offsetPWM, int16_t *minPWM,
int16_t *maxPWM) {
// Load data from the .CSV file
// We load it here instead of interpolation step as this will save time as we
// only open and load all the data once, savind time in intrepolation isnce we
// dont need to open the .CSV file over and over again.
_ForcePWMTable = _loadDataFromCSV(pathToCSVFile);

// Set correct parameters
// - Thruster Mapping
// - Thruster Direction
// - Offset PWM
// - Limit PWM
for (int8_t i = 0; i < 4; i++) {
_thrusterMapping[i] = thrusterMapping[i];
_thrusterDirection[i] = thrusterDirection[i];
Expand All @@ -190,10 +188,8 @@ void init(const std::string &pathToCSVFile, int8_t *thrusterMapping,
// The main core functionality of interacting and controling the thrusters
int16_t *drive_thrusters(float *thrusterForces) {
// Remap Thrusters
// From: [pin1:thruster1 , pin2:thruster2 , pin3:thruster3
// , pin4:thruster4 ] To: [pin1:<specifiedThruster>,
// pin2:<specifiedThruster>, pin3:<specifiedThruster>,
// pin4:<specifiedThruster>]
// From: [pin1:thruster1, pin2:thruster2, pin3:thruster3, pin4:thruster4]
// To: [pin1:<specifiedThruster>, pin2:<specifiedThruster>, pin3:<specifiedThruster>, pin4:<specifiedThruster>]
float thrusterForcesChangedMapping[4] = {0.0, 0.0, 0.0, 0.0};
for (int8_t pinNr = 0; pinNr < 4; pinNr++) {
int8_t remapedThrusterForcesIndex = _thrusterMapping[pinNr];
Expand All @@ -203,14 +199,14 @@ int16_t *drive_thrusters(float *thrusterForces) {

// Change direction of the thruster (Forward(+1)/Backwards(-1)) according to
// the direction parameter
float thrusterForcesCahngedDirection[4] = {0.0, 0.0, 0.0, 0.0};
float thrusterForcesChangedDirection[4] = {0.0, 0.0, 0.0, 0.0};
for (int8_t i = 0; i < 4; i++) {
thrusterForcesCahngedDirection[i] =
thrusterForcesChangedDirection[i] =
thrusterForcesChangedMapping[i] * _thrusterDirection[i];
}

// Interpolate forces to raw PWM values
int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesCahngedDirection);
int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesChangedDirection);

// Offset PWM
for (int8_t i = 0; i < 4; i++) {
Expand Down
23 changes: 11 additions & 12 deletions motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,9 @@
class ThrusterInterfaceASVNode : public rclcpp::Node {
private:
// ROS2 Variables ----------
// Creates ROS2 subscriber
rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr
_subscriberThrusterForces;
// Create ROS2 publisher
rclcpp::Publisher<std_msgs::msg::Int16MultiArray>::SharedPtr _publisherPWM;
// Create ROS2 timer/cycler
rclcpp::TimerBase::SharedPtr _thruster_driver_timer;

// Variables that are shared inside the object ----------
Expand All @@ -33,6 +30,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
// Publish PWM values for debuging purposes
std::shared_ptr<std_msgs::msg::Int16MultiArray> messagePWM =
std::make_shared<std_msgs::msg::Int16MultiArray>();

messagePWM->data.resize(4);
for (int8_t i = 0; i < 4; i++) {
messagePWM->data[i] = _pwm[i];
Expand All @@ -44,7 +42,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
// Builder for the object ----------
ThrusterInterfaceASVNode() : Node("thruster_interface_asv_node") {
// Thruster Driver Setup ----------
// Get filepath of .CSV file with ROS2 file path finder
// Get filepath of .CSV file with force mapping
std::string forcesToPWMDataFilePath =
ament_index_cpp::get_package_share_directory("thruster_interface_asv");
forcesToPWMDataFilePath += "/config/ThrustMe_P1000_force_mapping.csv";
Expand All @@ -53,6 +51,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
std::vector<int64_t> parameterThrusterMapping =
this->declare_parameter<std::vector<int64_t>>(
"propulsion.thrusters.thruster_to_pin_mapping", {0, 1, 2, 3});

int8_t ThrusterMapping[4] = {
static_cast<int8_t>(parameterThrusterMapping[0]),
static_cast<int8_t>(parameterThrusterMapping[1]),
Expand All @@ -63,6 +62,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
std::vector<int64_t> parameterThrusterDirection =
this->declare_parameter<std::vector<int64_t>>(
"propulsion.thrusters.thruster_direction", {1, 1, 1, 1});

int8_t thrusterDirection[4] = {
static_cast<int8_t>(parameterThrusterDirection[0]),
static_cast<int8_t>(parameterThrusterDirection[1]),
Expand All @@ -73,6 +73,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
std::vector<int64_t> parameterPWMOffset =
this->declare_parameter<std::vector<int64_t>>(
"propulsion.thrusters.thruster_PWM_offset", {0, 0, 0, 0});

int16_t offsetPWM[4] = {static_cast<int16_t>(parameterPWMOffset[0]),
static_cast<int16_t>(parameterPWMOffset[1]),
static_cast<int16_t>(parameterPWMOffset[2]),
Expand All @@ -82,13 +83,16 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
std::vector<int64_t> parameterPWMMin =
this->declare_parameter<std::vector<int64_t>>(
"propulsion.thrusters.thruster_PWM_min", {1100, 1100, 1100, 1100});

std::vector<int64_t> parameterPWMMax =
this->declare_parameter<std::vector<int64_t>>(
"propulsion.thrusters.thruster_PWM_max", {1900, 1900, 1900, 1900});

int16_t minPWM[4] = {static_cast<int16_t>(parameterPWMMin[0]),
static_cast<int16_t>(parameterPWMMin[1]),
static_cast<int16_t>(parameterPWMMin[2]),
static_cast<int16_t>(parameterPWMMin[3])};

int16_t maxPWM[4] = {static_cast<int16_t>(parameterPWMMax[0]),
static_cast<int16_t>(parameterPWMMax[1]),
static_cast<int16_t>(parameterPWMMax[2]),
Expand All @@ -100,18 +104,18 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
offsetPWM, minPWM, maxPWM);

// ROS Setup ----------
// Initialize ROS2 subscribers role
// Initialize ROS2 thrusterForces subscriber
_subscriberThrusterForces =
this->create_subscription<std_msgs::msg::Float32MultiArray>(
"/thrust/thruster_forces", 5,
std::bind(&ThrusterInterfaceASVNode::_thruster_forces_callback,
this, std::placeholders::_1));

// Initialize ROS2 publisher role
// Initialize ROS2 pwm publisher
_publisherPWM =
this->create_publisher<std_msgs::msg::Int16MultiArray>("/pwm", 5);

// Initialize a never ending cycle that continuisly publishes and drives
// Initialize a never ending cycle that continuously publishes and drives
// thrusters depending on what the ThrusterForces value is set to from
// ThrusterForces subscriber
using namespace std::chrono_literals;
Expand All @@ -126,13 +130,8 @@ class ThrusterInterfaceASVNode : public rclcpp::Node {
};

int main(int argc, char *argv[]) {
// Start ROS2
rclcpp::init(argc, argv);

// Start infinite loop of ROS2 Node
rclcpp::spin(std::make_shared<ThrusterInterfaceASVNode>());

// Shut down ROS2
rclcpp::shutdown();
return 0;
}

0 comments on commit b3f4d9b

Please sign in to comment.