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

name

Browse Source
This commit is contained in:
acki
2025-10-15 18:01:46 +02:00
parent 09f2ffda8e
commit 9ef67d0d35
1 changed files with 63 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,66 +18,6 @@ namespace Modelec
request->bauds = get_parameter("baudrate").as_int();
request->serial_port = get_parameter("serial_port").as_string();
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
if (res->success)
{
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
},
options);
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]()
{
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
SetStart(true);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10);
@@ -140,6 +80,66 @@ namespace Modelec
SendOrder("START", {std::to_string(msg->data)});
}
});
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
if (res->success)
{
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
},
options);
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]()
{
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
SetStart(true);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
}
PCBOdoInterface::~PCBOdoInterface()
@@ -179,7 +179,9 @@ namespace Modelec
message.y = y;
message.theta = theta;
odo_pos_publisher_->publish(message);
if (odo_pos_publisher_) {
odo_pos_publisher_->publish(message);
}
}
else if (tokens[1] == "SPEED")
{