modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-03-18 21:50:36 +01:00
Code Issues Packages Projects Releases Wiki Activity

some bug

Browse Source 
but found a huge bug with serial comm :/
This commit is contained in:
acki
2025-05-07 13:06:21 -04:00
parent 3ea4608dbf
commit 8060e518c1
8 changed files with 48 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
simulated_pcb/odo.py
View File

@@ -96,25 +96,31 @@ class SimulatedPCB:
def handle_message(self, msg):
if msg == "GET;POS":
print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}')
self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n'.encode())
elif msg == "GET;SPEED":
print(f'[TX] SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}')
self.ser.write(f'SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n'.encode())
elif msg.startswith("GET;DIST;"):
n = int(msg.split(';')[2])
dist = self.tof.get(n, 0)
print(f'[TX] SET;DIST;{n};{dist}')
self.ser.write(f'SET;DIST;{n};{dist}\n'.encode())
elif msg == "GET;PID":
print(f'[TX] SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}')
self.ser.write(f'SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n'.encode())
elif msg.startswith("SET;PID"):
self.pid = list(map(int, msg.split(';')[2:5]))
self.pid = list(map(float, msg.split(';')[2:5]))
print(f'[TX] OK;PID;1')
self.ser.write(f'OK;PID;1\n'.encode())
elif msg.startswith("SET;START"):
self.start = msg.split(';')[2] == '1'
print(f'[TX] OK;START;1')
self.ser.write(f'OK;START;1\n'.encode())
elif msg.startswith("SET;WAYPOINT"):
@@ -137,6 +143,7 @@ class SimulatedPCB:
self.x = float(parts[2])
self.y = float(parts[3])
self.theta = float(parts[4])
print(f'[TX] OK;POS;1')
self.ser.write(b'OK;POS;1\n')
def stop(self):

3
src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
View File

@@ -130,6 +130,9 @@ private:
void ResolveSetPIDRequest(bool success);
void ResolveAddWaypointRequest(bool success);
int timeout_ms = 1000;
int attempt = 5;
public:
void SendToPCB(const std::string &data) const;
void SendToPCB(const std::string& order, const std::string& elem, const std::vector<std::string>& data = {}) const;

3
src/modelec_com/src/pcb_alim_interface.cpp
View File

@@ -38,9 +38,6 @@ namespace Modelec
{
if (res->success)
{
RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str());
RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str());
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;

3
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -38,9 +38,6 @@ namespace Modelec
{
if (res->success)
{
RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str());
RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str());
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;

3
src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
View File

@@ -4,6 +4,7 @@
#include <QPushButton>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QSpinBox>
#include <rclcpp/rclcpp.hpp>
@@ -33,7 +34,7 @@ namespace ModelecGUI {
QHBoxLayout *posLayout_;
QPushButton *askPID_;
QLineEdit *pPIDBox_, *iPIDBox_, *dPIDBox_;
QDoubleSpinBox *pPIDBox_, *iPIDBox_, *dPIDBox_;
QHBoxLayout *pidLayout_;
QPushButton *setPID_;

29
src/modelec_gui/src/pages/odo_page.cpp
View File

@@ -51,18 +51,35 @@ namespace ModelecGUI
if (auto res = response.get()) {
RCLCPP_INFO(node_->get_logger(), "PID values received: p=%f, i=%f, d=%f", res->p, res->i, res->d);
QMetaObject::invokeMethod(this, [this, res]() {
pPIDBox_->setText(QString("%1").arg(res->p));
iPIDBox_->setText(QString("%1").arg(res->i));
dPIDBox_->setText(QString("%1").arg(res->d));
pPIDBox_->setValue(res->p);
iPIDBox_->setValue(res->i);
dPIDBox_->setValue(res->d);
});
} else {
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request.");
}
});
});
pPIDBox_ = new QLineEdit("");
iPIDBox_ = new QLineEdit("");
dPIDBox_ = new QLineEdit("");
pPIDBox_ = new QDoubleSpinBox(this);
pPIDBox_->setMinimum(0);
pPIDBox_->setMaximum(30);
pPIDBox_->setSingleStep(.1);
pPIDBox_->setValue(0);
pPIDBox_->setDecimals(2);
iPIDBox_ = new QDoubleSpinBox(this);
iPIDBox_->setMinimum(0);
iPIDBox_->setMaximum(30);
iPIDBox_->setSingleStep(.1);
iPIDBox_->setValue(0);
iPIDBox_->setDecimals(2);
dPIDBox_ = new QDoubleSpinBox(this);
dPIDBox_->setMinimum(0);
dPIDBox_->setMaximum(30);
dPIDBox_->setSingleStep(.1);
dPIDBox_->setValue(0);
dPIDBox_->setDecimals(2);
pidLayout_ = new QHBoxLayout;
pidLayout_->addWidget(pPIDBox_);

6
src/modelec_strat/data/config.xml
View File

@@ -43,15 +43,17 @@
<pcb>
<pcb_odo>
<name>pcb_odo</name>
<port>/dev/pts/5</port>
<port>/dev/pts/7</port>
<baud_rate>115200</baud_rate>
</pcb_odo>
<pcb_alim>
<name>pcb_alim</name>
<port>/dev/pts/9</port>
<port>/dev/pts/11</port>
<baud_rate>115200</baud_rate>
</pcb_alim>
</pcb>
<timeout_ms>300</timeout_ms>
<attempt>5</attempt>
</usb>
<mission_score>
<banner>20</banner>

10
src/modelec_strat/src/pami_manager.cpp
View File

@@ -80,8 +80,14 @@ namespace Modelec
reset_strat_sub_ = create_subscription<std_msgs::msg::Empty>(
"/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr)
{
timer_add_->cancel();
timer_remove_->cancel();
if (timer_add_)
{
timer_add_->cancel();
}
if (timer_remove_)
{
timer_remove_->cancel();
}
});
}