Add CAN FD Support

[JWhitleyWork]

This adds the enable_can_fd flag to both the sender and receiver. When enabled, this publishes or subscribes to a new topic suffixed with _fd for CAN FD messages and sends/receives them to/from the attached SocketCAN device. Closes #21.

[FranzAlbers]

Thanks for working on this useful feature!

I have tested the current state of the draft with some CAN FD radar sensors. The following modifications were necessary to actually receive CAN FD Messages:

1. The RAW Socket Option CAN_RAW_FD_FRAMES (https://www.kernel.org/doc/html/latest/networking/can.html#socketcan-rawfd) needs to be set, probably in bind_can_socket() in socket_can_common.cpp:

    // Enable CAN FD support
    const int32_t enable_canfd = 1;
    if (0 != setsockopt(file_descriptor, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd))) {
      throw std::runtime_error{"Failed to enable CAN FD support"};
    }

2. The data field of the fd_frame_msg needs to be resized before it is filled in the receive() function to avoid a crash:

  ros2_socketcan_msgs::msg::Frame fd_frame_msg(rosidl_runtime_cpp::MessageInitialization::ZERO);
  fd_frame_msg.data.resize(64);

and it should be resized afterwards to take advantage of the bounded data array:

fd_frame_msg.data.resize(receive_id.length());

3. I was only able to receive CAN FD Frames, when I commented line 142 in socket_can_receiver_node (receive_id = receiver_->receive(frame_msg.data.data(), interval_ns_);) before. Otherwise, I received malformed packets:

albers@leaf-pc:~$ ros2 topic echo /from_can_bus_fd
1669815402.176876 [101]     recvUC: malformed packet received from vendor 1.16 state parse:HEARTBEAT <52545053 02010110 3cd01001 ae4a17bb 7c255598 09010800 6a5c8763 b029452d @0x38 07011c00 00000000 00002103 00000000 c92a0000 00000000 c92a0000 01000000> (note: maybe partially bswap'd) smid 0x7 flags 0x1 otnh 28 rid 0x0 wid 0x3210000 first 10953 last 10953
1669815402.176904 [101]     recvUC: malformed packet received from vendor 1.16 state header < @0x0 52545053 02010110 3cd01001 ae4a17bb 7c255598 09010800 6a5c8763 ebe0472d 15051400 00001000 00000000 03210000 00000000 ca2a0000> (note: maybe partially bswap'd) smid 0x52 flags 0x54 otnh 21328
1669815402.179657 [101]     recvUC: malformed packet received from vendor 1.16 state parse:HEARTBEAT <52545053 02010110 3cd01001 ae4a17bb 7c255598 09010800 6a5c8763 9ebafc2d @0x38 07031c00 00000000 00002103 00000000 c92a0000 00000000 cb2a0000 02000000> (note: maybe partially bswap'd) smid 0x7 flags 0x3 otnh 28 rid 0x0 wid 0x3210000 first 10953 last 10955

[JWhitleyWork]

@FranzAlbers Thank you so much for testing this and working out some of the bugs! I have implemented your suggestions in this branch. Unfortunately, your testing uncovered something that I expected but hoped was not the case: once a socket has been opened with FD options enabled, it is not possible to send/receive to/from that socket with standard frames and vice-versa. This makes standard and FD mutually-exclusive, which is annoying.

I did my best to minimize the API changes but there is one change to avoid awkward syntax that subtly breaks the existing non-ROS API. When creating a Sender, if I were to completely avoid API changes, calling the constructor would look like this for standard:

auto sender_ = SocketCanSender("can0");

While doing the same with an FD Sender would look like this:

auto sender_ = SocketCanSender("can0", CanId{}, true);

However, I figured that it is very unlikely that anyone is using the non-ROS library in this package and also using the "default CAN ID" functionality in Sender so I instead implemented the FD version as:

auto sender_ = SocketCanSender("can0", true);

This breaks the API for those using the non-ROS library in a very specific way but, as I said, I doubt this is a large number of users, if any.

Please test this branch again and report back, if you can and provide feedback.

[JWhitleyWork]

This still needs testing but I believe it is good to go otherwise.

[FranzAlbers]

I've mentioned an issue in code review, which you might have overseen @JWhitleyWork:

The socket_can_receiver_node currently publishes CAN FD Frames, but the fields id, data, is_extended and is_error are not set due to a mixup of receive_id and fd_receive_id variables in SocketCanReceiverNode::receive(). I think dismissing the fd_receive_id and using only receive_id should fix this issue.

[JWhitleyWork]

I've mentioned an issue in code review, which you might have overseen @JWhitleyWork:

The socket_can_receiver_node currently publishes CAN FD Frames, but the fields id, data, is_extended and is_error are not set due to a mixup of receive_id and fd_receive_id variables in SocketCanReceiverNode::receive(). I think dismissing the fd_receive_id and using only receive_id should fix this issue.

Sorry I missed this before. Please check again - should be resolved now.

[FranzAlbers]

Receiving CAN FD frames works now as expected. I've successfully received some CAN FD Frames from our radar sensors. the decoded data looks reasonable as well.

Sending CAN FD frames back to the bus also works, though I've only tested it from the command line.

Receiving and sending also still work for usual can_msgs/Frames.

Not sure if I am allowed to approve this, but I think this can be merged.

[JWhitleyWork]

Needs review by @wep21 or @kenji-miyake.

[kenji-miyake]

@JWhitleyWork Thank you for working on this, and also thank you @FranzAlbers @xbroquer for reviews. 🙏

Please let me confirm one thing just in case.
This PR won't affect the existing users who just use ros2_socketcan by launching nodes, right? (Seeing it only adds a new parameter and internal changes, I suppose yes.)

In that case, if it's okay for you and someone familiar with CAN FD, then it's okay for TIER IV and probably for AWF.

@mitsudome-r @xmfcx Please comment if you have any thoughts.

[wep21]

I have tested the feature with virtual can.

sudo ip link add dev vcan0 type vcan
sudo ip link set vcan0 mtu 72
sudo ip link set vcan0 up
cangen -f vcan0 -v vcan0
ros2 launch ros2_socketcan socket_can_bridge.launch.xml enable_can_fd:=true interface:=vcan0

❯ ros2 topic echo /from_can_bus_fd
header:
  stamp:
    sec: 1675479833
    nanosec: 474542736
  frame_id: can
id: 126
is_extended: false
is_error: false
len: 16
data:
- 195
- 11
- 6
- 109
- 148
- 193
- 127
- 66
- 195
- 11
- 6
- 109
- 148
- 193
- 127
- 66
---

It worked fine, but a console always printed the following warning.

[WARN] [1675479940.290283568] [socket_can_receiver]: Error receiving CAN FD message: vcan0 - CAN Receive Timeout

Is this a correct behavior?

[FranzAlbers]

It worked fine, but a console always printed the following warning.

[WARN] [1675479940.290283568] [socket_can_receiver]: Error receiving CAN FD message: vcan0 - CAN Receive Timeout

Is this a correct behavior?

That's correct behavior in this case because the gap time of cangen is 200ms by default, and the receive timeout of the ros2_socketcan receiver node is only 10ms by default.

Warnings will be gone if you set the cangen gap time to be shorter than the timeout receiver_interval_sec, for example, with:

cangen -f vcan0 -v vcan0 -g 9 # milliseconds

Or with:

ros2 launch ros2_socketcan socket_can_bridge.launch.xml enable_can_fd:=true interface:=vcan0 receiver_interval_sec:=0.25 # seconds

[wep21]

@FranzAlbers Thank you for your comment. It worked well with your suggestion.

[kenji-miyake]

Probably it looks to be okay. Waiting for comment on #28 (comment) from @JWhitleyWork.

[JWhitleyWork]

@JWhitleyWork Thank you for working on this, and also thank you @FranzAlbers @xbroquer for reviews. pray

Please let me confirm one thing just in case. This PR won't affect the existing users who just use ros2_socketcan by launching nodes, right? (Seeing it only adds a new parameter and internal changes, I suppose yes.)

In that case, if it's okay for you and someone familiar with CAN FD, then it's okay for TIER IV and probably for AWF.

@mitsudome-r @xmfcx Please comment if you have any thoughts.

That is correct. I specifically designed this to not affect existing "traditional" CAN users. In order to enable CANFD, you must add a new parameter and set it to True (it is False by default). I think this is ready to go.

[kenji-miyake]

Thank you. Let's merge it!