Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion .github/workflows/build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ jobs:
- ros: kilted
ubuntu: noble
- ros: rolling
ubuntu: noble
ubuntu: resolute
name: ROS 2 ${{ matrix.ros }}
container:
image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-${{ matrix.ubuntu }}:latest
Expand Down
2 changes: 1 addition & 1 deletion axiomatic_adapter/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.

cmake_minimum_required(VERSION 3.8)
cmake_minimum_required(VERSION 3.22)
project(axiomatic_adapter)

if(NOT CMAKE_CXX_STANDARD)
Expand Down
2 changes: 1 addition & 1 deletion axiomatic_adapter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@

<buildtool_depend>ament_cmake_ros</buildtool_depend>

<depend>boost</depend>
<depend>cli11</depend>
<depend>libboost-system-dev</depend>
<depend>socketcan_adapter</depend>

<test_depend>catch2</test_depend>
Expand Down
38 changes: 18 additions & 20 deletions axiomatic_adapter/src/axiomatic_adapter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,13 +23,13 @@
#include <iostream>
#include <mutex>
#include <string>
#include <system_error>
#include <thread>
#include <utility>
#include <vector>

#include <boost/asio.hpp>
#include <boost/asio/steady_timer.hpp>
#include <boost/system/error_code.hpp>

namespace polymath
{
Expand Down Expand Up @@ -69,12 +69,12 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
boost::asio::steady_timer timer(tcp_io_context_);
timer.expires_after(TCP_IP_CONNECTION_TIMEOUT_MS);

TCPSocketConnectionState connection_state{false, boost::asio::error::would_block};
TCPSocketConnectionState connection_state{false, std::make_error_code(std::errc::operation_would_block)};
std::mutex connection_state_mutex;

// Asynchronously attempt to connect
boost::asio::async_connect(
tcp_socket_, endpoints, [&](const boost::system::error_code & error, const boost::asio::ip::tcp::endpoint &) {
tcp_socket_, endpoints, [&](const std::error_code & error, const boost::asio::ip::tcp::endpoint &) {
std::lock_guard<std::mutex> guard(connection_state_mutex);
connection_state.error_code = error;
connection_state.connected = !error;
Expand All @@ -83,11 +83,11 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
});

// Set up a timer to cancel the operation if it exceeds the timeout
timer.async_wait([&](const boost::system::error_code & error) {
timer.async_wait([&](const std::error_code & error) {
if (!error) {
std::lock_guard<std::mutex> guard(connection_state_mutex);
if (!connection_state.connected) {
connection_state.error_code = boost::asio::error::timed_out;
connection_state.error_code = std::make_error_code(std::errc::timed_out);
tcp_socket_.cancel();
}
}
Expand All @@ -98,7 +98,7 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
tcp_io_context_.run();

// capture the error message and connection state
boost::system::error_code captured_error;
std::error_code captured_error;
bool is_connected;
{
std::lock_guard<std::mutex> guard(connection_state_mutex);
Expand All @@ -124,16 +124,14 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
bool closeSocket()
{
if (socket_state_ != TCPSocketState::CLOSED) {
boost::system::error_code error_code;
tcp_socket_.close(error_code);

if (error_code) {
std::cerr << "[ERROR] Failed to close TCP socket: " << error_code.message() << std::endl;
try {
tcp_socket_.close();
} catch (const std::exception & e) {
std::cerr << "[ERROR] Failed to close TCP socket: " << e.what() << std::endl;
return false;
} else {
socket_state_ = TCPSocketState::CLOSED;
return true;
}
socket_state_ = TCPSocketState::CLOSED;
return true;
}
return true;
}
Expand Down Expand Up @@ -184,15 +182,15 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
{
std::vector<uint8_t> data(1024, 0);
std::atomic<bool> data_received(false);
boost::system::error_code error_code;
std::error_code error_code;

// Set up the timer for timeout
boost::asio::steady_timer timer(tcp_io_context_);
timer.expires_after(receive_timeout_ms_);

// Start async receive operation
tcp_socket_.async_receive(
boost::asio::buffer(data), [&](const boost::system::error_code & error, std::size_t bytes_transferred) {
boost::asio::buffer(data), [&](const std::error_code & error, std::size_t bytes_transferred) {
error_code = error;
if (!error) {
data.resize(bytes_transferred);
Expand All @@ -202,9 +200,9 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
});

// Set up the timer to handle timeout cancellation
timer.async_wait([&](const boost::system::error_code & error) {
timer.async_wait([&](const std::error_code & error) {
if (!error && !data_received.load()) {
error_code = boost::asio::error::timed_out;
error_code = std::make_error_code(std::errc::timed_out);
// Cancel the ongoing async receive operation on timeout (does not close socket)
tcp_socket_.cancel();
}
Expand All @@ -215,7 +213,7 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
tcp_io_context_.run();

// Check for timeout or other errors
if (error_code == boost::asio::error::timed_out) {
if (error_code == std::errc::timed_out) {
return std::optional<AxiomaticAdapter::socket_error_string_t>("Receive operation timed out");
} else if (error_code) {
return std::optional<AxiomaticAdapter::socket_error_string_t>(
Expand Down Expand Up @@ -353,7 +351,7 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
struct TCPSocketConnectionState
{
bool connected{false};
boost::system::error_code error_code{boost::asio::error::would_block};
std::error_code error_code{std::make_error_code(std::errc::operation_would_block)};
};

boost::asio::io_context tcp_io_context_;
Expand Down
2 changes: 1 addition & 1 deletion socketcan_adapter/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.

cmake_minimum_required(VERSION 3.8)
cmake_minimum_required(VERSION 3.22)
project(socketcan_adapter)

# Project-wide language standards
Expand Down
2 changes: 1 addition & 1 deletion socketcan_adapter_ros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.

cmake_minimum_required(VERSION 3.8)
cmake_minimum_required(VERSION 3.22)
project(socketcan_adapter_ros)

# Project-wide language standards
Expand Down
Loading