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

emergency topic alim pcb

Browse Source
This commit is contained in:
acki
2025-04-21 23:54:35 -04:00
parent e047f94ef2
commit 575fbfee79
6 changed files with 78 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modelec/include/modelec/pcb_alim_interface.hpp
View File

@@ -7,6 +7,7 @@
#include <modelec_interface/srv/alim_bau.hpp>
#include <modelec_interface/srv/alim_emg.hpp>
#include <modelec_interface/srv/alim_temp.hpp>
#include <modelec_interface/msg/alim_emg.hpp>
namespace Modelec
{
@@ -38,6 +39,10 @@ private:
void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
rclcpp::Subscription<modelec_interface::msg::AlimEmg>::SharedPtr pcb_emg_subscriber_;
void PCBEmgCallback(const modelec_interface::msg::AlimEmg::SharedPtr msg) const;
rclcpp::Service<modelec_interface::srv::AlimOut>::SharedPtr pcb_out_service_;
rclcpp::Service<modelec_interface::srv::AlimIn>::SharedPtr pcb_in_service_;
rclcpp::Service<modelec_interface::srv::AlimBau>::SharedPtr pcb_bau_service_;
@@ -87,7 +92,7 @@ private:
void ResolveSetPCBOutRequest(const PCBData& value);
void ResolveGetPCBInRequest(const PCBData& value);
void ResolveGetPCBBauRequest(const PCBBau& value);
void ResolveSetPCBEmgRequest(const PCBData& value);
void ResolveSetPCBEmgRequest(bool value);
void ResolveGetPCBTempRequest(const PCBData& value);
public:

5
src/modelec/launch/test.modelec.launch.yml
View File

@@ -10,6 +10,11 @@ launch:
exec: 'pcb_odo_interface'
name: 'pcb_odo_interface'
- node:
pkg: 'modelec'
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"

73
src/modelec/src/pcb_alim_interface.cpp
View File

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
request->name = "pcb_alim";
request->bauds = 115200;
request->serial_port = "/dev/serial0"; // TODO : check the real serial port
request->serial_port = "/dev/pts/12"; // TODO : check the real serial port
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
@@ -38,7 +38,7 @@ namespace Modelec
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
result.get()->publisher, 10,
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
@@ -52,7 +52,7 @@ namespace Modelec
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
}
else
{
@@ -115,6 +115,13 @@ namespace Modelec
{
HandleGetPCBTempData(request, response);
});
pcb_emg_subscriber_ = this->create_subscription<modelec_interface::msg::AlimEmg>(
"alim/emg", 10,
[this](const modelec_interface::msg::AlimEmg::SharedPtr msg)
{
PCBEmgCallback(msg);
});
}
PCBAlimInterface::~PCBAlimInterface()
@@ -175,8 +182,7 @@ namespace Modelec
else if (tokens[1] == "EMG")
{
bool success = true;
bool value = (tokens[3] == "1");
ResolveSetPCBEmgRequest({success, value});
ResolveSetPCBEmgRequest(success);
}
}
else if (tokens[0] == "KO")
@@ -191,9 +197,8 @@ namespace Modelec
else if (tokens[1] == "EMG")
{
bool success = false;
int value = -1;
ResolveSetPCBEmgRequest({success, value});
ResolveSetPCBEmgRequest(success);
}
}
else
@@ -202,6 +207,11 @@ namespace Modelec
}
}
void PCBAlimInterface::PCBEmgCallback(const modelec_interface::msg::AlimEmg::SharedPtr msg) const
{
SendOrder("EMG", {"STATE", msg->activate == true ? "1" : "0"});
}
void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
{
@@ -259,7 +269,7 @@ namespace Modelec
response->result = result.value;
}
void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interface::srv::AlimBau::Request> request,
void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interface::srv::AlimBau::Request>,
std::shared_ptr<modelec_interface::srv::AlimBau::Response> response)
{
std::promise<PCBBau> promise;
@@ -294,7 +304,7 @@ namespace Modelec
}
void PCBAlimInterface::HandleGetPCBTempData(
const std::shared_ptr<modelec_interface::srv::AlimTemp::Request> request,
const std::shared_ptr<modelec_interface::srv::AlimTemp::Request>,
std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response)
{
std::promise<PCBData> promise;
@@ -357,6 +367,51 @@ namespace Modelec
}
}
void PCBAlimInterface::ResolveGetPCBBauRequest(const PCBBau& value)
{
std::lock_guard<std::mutex> lock(pcb_bau_mutex_);
if (!pcb_bau_promises_.empty())
{
auto promise = std::move(pcb_bau_promises_.front());
pcb_bau_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB bau data");
}
}
void PCBAlimInterface::ResolveSetPCBEmgRequest(bool value)
{
std::lock_guard<std::mutex> lock(pcb_emg_mutex_);
if (!pcb_emg_promises_.empty())
{
auto promise = std::move(pcb_emg_promises_.front());
pcb_emg_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB emg data");
}
}
void PCBAlimInterface::ResolveGetPCBTempRequest(const PCBData& value)
{
std::lock_guard<std::mutex> lock(pcb_temp_mutex_);
if (!pcb_temp_promises_.empty())
{
auto promise = std::move(pcb_temp_promises_.front());
pcb_temp_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB temp data");
}
}
void PCBAlimInterface::SendToPCB(const std::string& data) const
{
auto message = std_msgs::msg::String();

2
src/modelec/src/pcb_odo_interface.cpp
View File

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
request->name = "pcb_odo";
request->bauds = 115200;
request->serial_port = "/dev/pts/4"; // TODO : check the real serial port
request->serial_port = "/dev/pts/7"; // TODO : check the real serial port
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{

1
src/modelec_interface/CMakeLists.txt
View File

@@ -11,6 +11,7 @@ find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Alim/AlimEmg.msg"
"msg/Odometry/OdometryPos.msg"
"msg/Odometry/OdometrySpeed.msg"
"msg/Odometry/OdometryToF.msg"

1
src/modelec_interface/msg/Alim/AlimEmg.msg Normal file
View File

@@ -0,0 +1 @@
bool activate