diff --git a/udp_driver/include/udp_driver/udp_socket.hpp b/udp_driver/include/udp_driver/udp_socket.hpp index 14a84fe6..1af138ca 100644 --- a/udp_driver/include/udp_driver/udp_socket.hpp +++ b/udp_driver/include/udp_driver/udp_socket.hpp @@ -59,6 +59,7 @@ class UdpSocket void close(); bool isOpen() const; void bind(); + void joinMulticast(std::string & multicast_addr); /* * Blocking Send Operation diff --git a/udp_driver/params/example_udp_multicast_params.yml b/udp_driver/params/example_udp_multicast_params.yml new file mode 100644 index 00000000..2d569572 --- /dev/null +++ b/udp_driver/params/example_udp_multicast_params.yml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + ip: "239.255.0.1" + port: 22017 diff --git a/udp_driver/src/udp_receiver_node.cpp b/udp_driver/src/udp_receiver_node.cpp index 2a542f82..13229699 100644 --- a/udp_driver/src/udp_receiver_node.cpp +++ b/udp_driver/src/udp_receiver_node.cpp @@ -62,6 +62,7 @@ LNI::CallbackReturn UdpReceiverNode::on_configure(const lc::State & state) m_udp_driver->init_receiver(m_ip, m_port); m_udp_driver->receiver()->open(); m_udp_driver->receiver()->bind(); + m_udp_driver->receiver()->joinMulticast(m_ip); m_udp_driver->receiver()->asyncReceive( std::bind(&UdpReceiverNode::receiver_callback, this, std::placeholders::_1)); } catch (const std::exception & ex) { diff --git a/udp_driver/src/udp_socket.cpp b/udp_driver/src/udp_socket.cpp index 619c7be0..74375486 100644 --- a/udp_driver/src/udp_socket.cpp +++ b/udp_driver/src/udp_socket.cpp @@ -192,5 +192,18 @@ void UdpSocket::bind() m_udp_socket.bind(m_host_endpoint); } +void UdpSocket::joinMulticast(std::string & multicast_addr) +{ + if (address::from_string(multicast_addr).is_v4() && + address::from_string(multicast_addr).to_v4().is_multicast()) + { + m_udp_socket.set_option( + asio::ip::multicast::join_group(address::from_string(multicast_addr).to_v4())); + RCLCPP_INFO( + rclcpp::get_logger("UdpSocket::multicast"), + "Joined multicast group: %s", multicast_addr.c_str()); + } +} + } // namespace udp_driver } // namespace drivers