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

clean exit if can't open pcb

Browse Source
This commit is contained in:
acki
2025-05-20 20:12:08 -04:00
parent 6c4b96a62b
commit dad30740be
6 changed files with 111 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/modelec_com/include/modelec_com/pcb_action_interface.hpp
View File

@@ -60,6 +60,8 @@ namespace Modelec
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_pub_;
bool isOk = false;
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& order, const std::string& elem,

2
src/modelec_com/include/modelec_com/pcb_alim_interface.hpp
View File

@@ -96,6 +96,8 @@ namespace Modelec
void ResolveSetPCBEmgRequest(bool value);
void ResolveGetPCBTempRequest(const PCBData& value);
bool isOk = false;
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& order, const std::string& elem,

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

@@ -135,6 +135,8 @@ namespace Modelec
int timeout_ms = 1000;
int attempt = 5;
bool isOk = false;
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& order, const std::string& elem,

7
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -58,6 +58,8 @@ namespace Modelec
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
}
else
{
@@ -214,7 +216,10 @@ namespace Modelec
PCBActionInterface::~PCBActionInterface()
{
pcb_executor_->cancel();
if (pcb_executor_)
{
pcb_executor_->cancel();
}
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();

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

@@ -58,6 +58,8 @@ namespace Modelec
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
}
else
{
@@ -131,7 +133,10 @@ namespace Modelec
PCBAlimInterface::~PCBAlimInterface()
{
pcb_executor_->cancel();
if (pcb_executor_)
{
pcb_executor_->cancel();
}
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();
@@ -220,6 +225,13 @@ namespace Modelec
void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();
@@ -239,6 +251,13 @@ namespace Modelec
void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();
@@ -258,6 +277,13 @@ namespace Modelec
void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr<modelec_interfaces::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimIn::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();
@@ -277,6 +303,13 @@ namespace Modelec
void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interfaces::srv::AlimBau::Request>,
std::shared_ptr<modelec_interfaces::srv::AlimBau::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBBau> promise;
auto future = promise.get_future();
@@ -295,6 +328,13 @@ namespace Modelec
void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr<modelec_interfaces::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimEmg::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<bool> promise;
auto future = promise.get_future();
@@ -312,6 +352,13 @@ namespace Modelec
const std::shared_ptr<modelec_interfaces::srv::AlimTemp::Request>,
std::shared_ptr<modelec_interfaces::srv::AlimTemp::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();

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

@@ -60,6 +60,8 @@ namespace Modelec
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
}
else
{
@@ -172,7 +174,10 @@ namespace Modelec
PCBOdoInterface::~PCBOdoInterface()
{
pcb_executor_->cancel();
if (pcb_executor_)
{
pcb_executor_->cancel();
}
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();
@@ -327,6 +332,12 @@ namespace Modelec
const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<long> promise;
auto future = promise.get_future();
@@ -344,6 +355,12 @@ namespace Modelec
const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<OdometryData> promise;
auto future = promise.get_future();
@@ -364,6 +381,12 @@ namespace Modelec
const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<OdometryData> promise;
auto future = promise.get_future();
@@ -383,6 +406,13 @@ namespace Modelec
void PCBOdoInterface::HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<bool> promise;
auto future = promise.get_future();
@@ -398,6 +428,12 @@ namespace Modelec
void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<PIDData> promise;
auto future = promise.get_future();
@@ -418,6 +454,13 @@ namespace Modelec
void PCBOdoInterface::HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<bool> promise;
auto future = promise.get_future();
@@ -436,6 +479,13 @@ namespace Modelec
const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<bool> promise;
auto future = promise.get_future();