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
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,7 @@ class SOCKETCAN_PUBLIC SocketCanReceiver
{
public:
/// Constructor
explicit SocketCanReceiver(
const std::string & interface = "can0", const bool enable_fd = false,
const bool enable_loopback = false);
explicit SocketCanReceiver(const std::string & interface = "can0", const bool enable_fd = false);
/// Destructor
~SocketCanReceiver() noexcept;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,6 @@ class SOCKETCAN_PUBLIC SocketCanReceiverNode final
std::chrono::nanoseconds interval_ns_;
bool enable_fd_;
bool use_bus_time_;
bool enable_loopback_;
};
} // namespace socketcan
} // namespace drivers
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ class SOCKETCAN_PUBLIC SocketCanSender
explicit SocketCanSender(
const std::string & interface = "can0",
const bool enable_fd = false,
const bool enable_loopback = false,
const CanId & default_id = CanId{});
/// Destructor
~SocketCanSender() noexcept;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@ class SOCKETCAN_PUBLIC SocketCanSenderNode final
private:
std::string interface_;
bool enable_fd_;
bool enable_loopback_;
rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr frames_sub_;
rclcpp::Subscription<ros2_socketcan_msgs::msg::FdFrame>::SharedPtr fd_frames_sub_;
std::unique_ptr<SocketCanSender> sender_;
Expand Down
2 changes: 1 addition & 1 deletion ros2_socketcan/launch/socket_can_bridge.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
<arg name="interface" value="$(var interface)" />
<arg name="interval_sec" value="$(var receiver_interval_sec)" />
<arg name="enable_can_fd" value="$(var enable_can_fd)" />
<arg name="enable_frame_loopback" value="$(var enable_frame_loopback)" />
<arg name="from_can_bus_topic" value="$(var from_can_bus_topic)" />
<arg name="filters" value="$(var filters)" />
<arg name="use_bus_time" value="$(var use_bus_time)" />
Expand All @@ -24,6 +23,7 @@
<arg name="interface" value="$(var interface)" />
<arg name="timeout_sec" value="$(var sender_timeout_sec)" />
<arg name="enable_can_fd" value="$(var enable_can_fd)" />
<arg name="enable_frame_loopback" value="$(var enable_frame_loopback)" />
<arg name="to_can_bus_topic" value="$(var to_can_bus_topic)" />
</include>

Expand Down
2 changes: 0 additions & 2 deletions ros2_socketcan/launch/socket_can_receiver.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@ def generate_launch_description():
parameters=[{
'interface': LaunchConfiguration('interface'),
'enable_can_fd': LaunchConfiguration('enable_can_fd'),
'enable_frame_loopback': LaunchConfiguration('enable_frame_loopback'),
'interval_sec':
LaunchConfiguration('interval_sec'),
'filters': LaunchConfiguration('filters'),
Expand Down Expand Up @@ -82,7 +81,6 @@ def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('interface', default_value='can0'),
DeclareLaunchArgument('enable_can_fd', default_value='false'),
DeclareLaunchArgument('enable_frame_loopback', default_value='false'),
DeclareLaunchArgument('interval_sec', default_value='0.01'),
DeclareLaunchArgument('use_bus_time', default_value='false'),
DeclareLaunchArgument('filters', default_value='0:0',
Expand Down
2 changes: 2 additions & 0 deletions ros2_socketcan/launch/socket_can_sender.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ def generate_launch_description():
parameters=[{
'interface': LaunchConfiguration('interface'),
'enable_can_fd': LaunchConfiguration('enable_can_fd'),
'enable_frame_loopback': LaunchConfiguration('enable_frame_loopback'),
'timeout_sec':
LaunchConfiguration('timeout_sec'),
}],
Expand Down Expand Up @@ -79,6 +80,7 @@ def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('interface', default_value='can0'),
DeclareLaunchArgument('enable_can_fd', default_value='false'),
DeclareLaunchArgument('enable_frame_loopback', default_value='false'),
DeclareLaunchArgument('timeout_sec', default_value='0.01'),
DeclareLaunchArgument('auto_configure', default_value='true'),
DeclareLaunchArgument('auto_activate', default_value='true'),
Expand Down
6 changes: 2 additions & 4 deletions ros2_socketcan/src/socket_can_receiver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,8 @@ namespace socketcan
{

////////////////////////////////////////////////////////////////////////////////
SocketCanReceiver::SocketCanReceiver(
const std::string & interface, const bool enable_fd,
const bool enable_loopback)
: m_file_descriptor{bind_can_socket(interface, enable_fd, enable_loopback)},
SocketCanReceiver::SocketCanReceiver(const std::string & interface, const bool enable_fd)
: m_file_descriptor{bind_can_socket(interface, enable_fd)},
m_enable_fd(enable_fd)
{
}
Expand Down
6 changes: 1 addition & 5 deletions ros2_socketcan/src/socket_can_receiver_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@ SocketCanReceiverNode::SocketCanReceiverNode(rclcpp::NodeOptions options)
interface_ = this->declare_parameter("interface", "can0");
use_bus_time_ = this->declare_parameter<bool>("use_bus_time", false);
enable_fd_ = this->declare_parameter<bool>("enable_can_fd", false);
enable_loopback_ = this->declare_parameter<bool>("enable_frame_loopback", false);
double interval_sec = this->declare_parameter("interval_sec", 0.01);
this->declare_parameter("filters", "0:0");
interval_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
Expand All @@ -47,9 +46,6 @@ SocketCanReceiverNode::SocketCanReceiverNode(rclcpp::NodeOptions options)
RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str());
RCLCPP_INFO(this->get_logger(), "use bus time: %d", use_bus_time_);
RCLCPP_INFO(this->get_logger(), "can fd enabled: %s", enable_fd_ ? "true" : "false");
RCLCPP_INFO(
this->get_logger(), "frame loopback enabled: %s",
enable_loopback_ ? "true" : "false");
RCLCPP_INFO(this->get_logger(), "interval(s): %f", interval_sec);
}

Expand All @@ -58,7 +54,7 @@ LNI::CallbackReturn SocketCanReceiverNode::on_configure(const lc::State & state)
(void)state;

try {
receiver_ = std::make_unique<SocketCanReceiver>(interface_, enable_fd_, enable_loopback_);
receiver_ = std::make_unique<SocketCanReceiver>(interface_, enable_fd_);
// apply CAN filters
auto filters = get_parameter("filters").as_string();
receiver_->SetCanFilters(SocketCanReceiver::CanFilterList(filters));
Expand Down
3 changes: 2 additions & 1 deletion ros2_socketcan/src/socket_can_sender.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,9 +36,10 @@ namespace socketcan
SocketCanSender::SocketCanSender(
const std::string & interface,
const bool enable_fd,
const bool enable_loopback,
const CanId & default_id)
: m_enable_fd(enable_fd),
m_file_descriptor{bind_can_socket(interface, m_enable_fd)},
m_file_descriptor{bind_can_socket(interface, m_enable_fd, enable_loopback)},
m_default_id{default_id}
{
}
Expand Down
6 changes: 5 additions & 1 deletion ros2_socketcan/src/socket_can_sender_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,16 @@ SocketCanSenderNode::SocketCanSenderNode(rclcpp::NodeOptions options)
{
interface_ = this->declare_parameter("interface", "can0");
enable_fd_ = this->declare_parameter("enable_can_fd", false);
enable_loopback_ = this->declare_parameter<bool>("enable_frame_loopback", false);
double timeout_sec = this->declare_parameter("timeout_sec", 0.01);
timeout_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(timeout_sec));

RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str());
RCLCPP_INFO(this->get_logger(), "can fd enabled: %s", enable_fd_ ? "true" : "false");
RCLCPP_INFO(
this->get_logger(), "frame loopback enabled: %s",
enable_loopback_ ? "true" : "false");
RCLCPP_INFO(this->get_logger(), "timeout(s): %f", timeout_sec);
}

Expand All @@ -49,7 +53,7 @@ LNI::CallbackReturn SocketCanSenderNode::on_configure(const lc::State & state)
(void)state;

try {
sender_ = std::make_unique<SocketCanSender>(interface_, enable_fd_);
sender_ = std::make_unique<SocketCanSender>(interface_, enable_fd_, enable_loopback_);
} catch (const std::exception & ex) {
RCLCPP_ERROR(
this->get_logger(), "Error opening CAN sender: %s - %s",
Expand Down