Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update threading for multiple vehicle support. #8

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Update threading for multiple vehicle support. #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion src/link/udp_link.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,9 @@ using boost::asio::ip::udp;
using std::placeholders::_1;
using std::placeholders::_2;

UDPLink::UDPLink(std::string host, int port)
UDPLink::UDPLink(std::string host, int port, SynapseRos* ros)
{
ros_ = ros;
remote_endpoint_ = *udp::resolver(io_context_).resolve(udp::resolver::query(host, std::to_string(port)));
my_endpoint_ = udp::endpoint(udp::v4(), 4242);

Expand Down
4 changes: 2 additions & 2 deletions src/link/udp_link.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,12 +23,12 @@ class UDPLink {
boost::asio::ip::udp::endpoint my_endpoint_;

public:
SynapseRos* ros_ { NULL };
UDPLink(std::string host, int port);
UDPLink(std::string host, int port, SynapseRos* ros);
void run_for(std::chrono::seconds sec);
void write(const uint8_t* buf, uint32_t len);

private:
SynapseRos* ros_;
void timeout_handler();
void tx_handler(const boost::system::error_code& error, std::size_t bytes_transferred);
void rx_handler(const boost::system::error_code& error, std::size_t bytes_transferred);
Expand Down
26 changes: 13 additions & 13 deletions src/synapse_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,17 +15,10 @@
#include <synapse_pb/nav_sat_fix.pb.h>
#include <synapse_pb/wheel_odometry.pb.h>

using namespace std::chrono_literals;
using namespace google::protobuf::util;

using std::placeholders::_1;
std::shared_ptr<UDPLink> g_udp_link { NULL };

void udp_entry_point()
{
while (rclcpp::ok()) {
g_udp_link->run_for(std::chrono::seconds(1));
}
}

SynapseRos::SynapseRos()
: Node("synapse_ros")
Expand Down Expand Up @@ -85,9 +78,16 @@ SynapseRos::SynapseRos()
pub_clock_offset_ = this->create_publisher<builtin_interfaces::msg::Time>("out/clock_offset", 10);

// create udp link
g_udp_link = std::make_shared<UDPLink>(host, port);
g_udp_link.get()->ros_ = this;
udp_thread_ = std::make_shared<std::thread>(udp_entry_point);
udp_link_ = std::make_shared<UDPLink>(host, port, this);
udp_thread_ = std::make_shared<std::thread>(std::bind(
&SynapseRos::udp_run, this));
}

void SynapseRos::udp_run()
{
while (rclcpp::ok()) {
udp_link_->run_for(1s);
}
}

SynapseRos::~SynapseRos()
Expand Down Expand Up @@ -510,8 +510,8 @@ void SynapseRos::udp_send(const synapse_pb::Frame& frame) const
std::cerr << "Failed to serialize " << frame.msg_case() << std::endl;
return;
}
if (g_udp_link != nullptr) {
g_udp_link.get()->write((const uint8_t*)stream.str().c_str(), stream.str().length());
if (udp_link_.get() != nullptr) {
udp_link_->write((const uint8_t*)stream.str().c_str(), stream.str().length());
}
}

Expand Down
6 changes: 4 additions & 2 deletions src/synapse_ros.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#include <synapse_pb/twist.pb.h>
#include <synapse_pb/wheel_odometry.pb.h>

class UdpClient;
class UDPLink;

void udp_entry_point();

Expand All @@ -52,6 +52,7 @@ class SynapseRos : public rclcpp::Node {
private:
builtin_interfaces::msg::Time ros_clock_offset_ {};
builtin_interfaces::msg::Time compute_stamp(const synapse_pb::Timestamp& msg);
void udp_run(void);

// subscriptions ros -> cerebri
rclcpp::Subscription<actuator_msgs::msg::Actuators>::SharedPtr sub_actuators_;
Expand Down Expand Up @@ -88,8 +89,9 @@ class SynapseRos : public rclcpp::Node {
rclcpp::Publisher<builtin_interfaces::msg::Time>::SharedPtr pub_uptime_;
rclcpp::Publisher<builtin_interfaces::msg::Time>::SharedPtr pub_clock_offset_;

// callbacks
// threading
std::shared_ptr<std::thread> udp_thread_;
std::shared_ptr<UDPLink> udp_link_;
};

// vi: ts=4 sw=4 et
Expand Down