リベース

Author: HansRobo

crane_robot_receiver/src/robot_receiver_node.cpp

@@ -4,7 +4,16 @@
 // license that can be found in the LICENSE file or at
 // https://opensource.org/licenses/MIT.
 
-#include <atomic>
+//#include <arpa/inet.h>
+//#include <netinet/in.h>
+//#include <stdio.h>
+//#include <stdlib.h>
+//#include <string.h>
+//#include <sys/socket.h>
+//#include <sys/types.h>
+//#include <time.h>
+
+//#include <atomic>
 #include <boost/asio.hpp>
 #include <boost/thread.hpp>
 #include <crane_msgs/msg/robot_feedback.hpp>
@@ -24,7 +33,7 @@ struct RobotInterfaceConfig
 auto makeConfig(uint8_t id) -> RobotInterfaceConfig
 {
   RobotInterfaceConfig config;
-  config.ip = "224.5.20." + std::to_string(id + 100);
+  config.ip = "224.5.20.10" + std::to_string(id);
   config.port = 50100 + id;
   return config;
 }
@@ -57,39 +66,43 @@ union FloatUnion {
   char b[4];
 };
 
-struct RobotInterface
+class MulticastReceiver
 {
-  explicit RobotInterface(const RobotInterfaceConfig & config)
-  : socket(io_context, udp::endpoint(udp::v4(), config.port)), config(config)
-  {
-    open();
-  }
-
-  ~RobotInterface() { socket.close(); }
-
-  void open()
+public:
+  MulticastReceiver(const std::string & host, const int port)
+  : socket(io_service, boost::asio::ip::udp::v4()), buffer(2048)
   {
-    socket.open(endpoint.protocol());
-    socket.set_option(udp::socket::reuse_address(true));
-    socket.bind(endpoint);
-
-    auto address = boost::asio::ip::make_address(config.ip);
-    socket.set_option(boost::asio::ip::multicast::join_group(address));
+    boost::asio::ip::address addr = boost::asio::ip::address::from_string(host);
+    if (!addr.is_multicast()) {
+      throw std::runtime_error("expected multicast address");
+    }
 
-    socket.async_receive_from(
-      boost::asio::buffer(buffer, max_length), sender_endpoint,
-      std::bind(&RobotInterface::handle_receive, this, _1, _2));
+    socket.set_option(boost::asio::socket_base::reuse_address(true));
+    socket.set_option(boost::asio::ip::multicast::join_group(addr.to_v4()));
+    socket.bind(boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), port));
+    socket.non_blocking(true);
   }
 
-  void handle_receive(const boost::system::error_code & error, [[maybe_unused]] size_t bytes)
+  bool receive()
   {
-    if (error) {
-      return;
+    if (socket.available()) {
+      boost::system::error_code error;
+      received_size = socket.receive(boost::asio::buffer(buffer), 0, error);
+      if (error && error != boost::asio::error::message_size) {
+        throw boost::system::system_error(error);
+        return false;
+      }
+      return true;
+    } else {
+      return false;
     }
+  }
 
+  void updateFeedback()
+  {
     FloatUnion float_union;
     RobotInfoUnion robot_info_union;
-    robot_info_union.data = robot_feedback.load();
+    robot_info_union.data = robot_feedback;
 
     // 0,1byte目は識別子みたいな感じ
     auto header = buffer[2];
@@ -189,44 +202,43 @@ struct RobotInterface
       default:
         break;
     }
-    robot_feedback.store(robot_info_union.data);
+    robot_feedback = robot_info_union.data;
   }
 
-  // この変数は非同期で読み書きするのでatomicにする
-  std::atomic<RobotFeedback> robot_feedback;
-
-  boost::asio::io_context io_context;
+  RobotFeedback getFeedback() { return robot_feedback; }
 
-  udp::socket socket;
+private:
+  boost::asio::io_service io_service;
 
-  udp::endpoint sender_endpoint;
+  boost::asio::ip::udp::socket socket;
 
-  enum { max_length = 1024 };
+  std::vector<char> buffer;
 
-  char buffer[max_length]{};
+  size_t received_size;
 
-  udp::endpoint endpoint;
-
-  const RobotInterfaceConfig config;
+  RobotFeedback robot_feedback;
 };
 
 class RobotReceiverNode : public rclcpp::Node
 {
 public:
-  RobotReceiverNode(uint8_t robot_num = 6) : rclcpp::Node("robot_receiver_node")
+  RobotReceiverNode(uint8_t robot_num = 10) : rclcpp::Node("robot_receiver_node")
   {
     publisher = create_publisher<crane_msgs::msg::RobotFeedbackArray>("/robot_feedback", 10);
 
     for (int i = 0; i < robot_num; i++) {
       auto config = makeConfig(i);
-      robot_interfaces.push_back(std::make_shared<RobotInterface>(config));
+      receivers.push_back(std::make_shared<MulticastReceiver>(config.ip, config.port));
     }
 
     using namespace std::chrono_literals;
     timer = create_wall_timer(10ms, [&]() {
       crane_msgs::msg::RobotFeedbackArray msg;
-      for (auto & robot_interface : robot_interfaces) {
-        auto robot_feedback = robot_interface->robot_feedback.load();
+      for (auto & receiver : receivers) {
+        if (receiver->receive()) {
+          receiver->updateFeedback();
+        }
+        auto robot_feedback = receiver->getFeedback();
         crane_msgs::msg::RobotFeedback robot_feedback_msg;
         robot_feedback_msg.robot_id = robot_feedback.head[0];
         robot_feedback_msg.counter = robot_feedback.counter;
@@ -267,7 +279,7 @@ class RobotReceiverNode : public rclcpp::Node
 
   rclcpp::TimerBase::SharedPtr timer;
 
-  std::vector<std::shared_ptr<RobotInterface>> robot_interfaces;
+  std::vector<std::shared_ptr<MulticastReceiver>> receivers;
 
   rclcpp::Publisher<crane_msgs::msg::RobotFeedbackArray>::SharedPtr publisher;
 };
@@ -281,5 +293,40 @@ int main(int argc, char * argv[])
   exe.add_node(node->get_node_base_interface());
   exe.spin();
   rclcpp::shutdown();
+  //    try {
+  //      boost::asio::io_service io_service;
+  //
+  //      // ローカルホストの指定したポートでUDPソケットを開く
+  //      udp::socket socket(io_service);
+  //      auto listen_address = boost::asio::ip::address::from_string("224.5.20.100");
+  //      boost::asio::ip::udp::endpoint listen_endpoint(listen_address, 50100);
+  //
+  //      socket.open(listen_endpoint.protocol());
+  //
+  //      if (socket.is_open()) {
+  //        std::cout << "socket opened" << std::endl;
+  //      } else {
+  //        std::cout << "socket not opened" << std::endl;
+  //      }
+  //      socket.set_option(boost::asio::ip::udp::socket::reuse_address(true));
+  //      socket.bind(listen_endpoint);
+  //
+  //      // Join the multicast group.
+  //      socket.set_option(boost::asio::ip::multicast::join_group(listen_address));
+  //
+  //
+  //      std::cout << "joined multicast group" << std::endl;
+  //
+  //      for (;;) {  // 無限ループでパケットを待ち受ける
+  //        char data[1024];
+  //        udp::endpoint sender_endpoint;
+  //        size_t length = socket.receive_from(boost::asio::buffer(data), sender_endpoint);
+  //
+  //        std::cout << "Received packet: " << std::string(data, length) << std::endl;
+  //      }
+  //    } catch (std::exception & e) {
+  //      std::cerr << "Exception: " << e.what() << "\n";
+  //    }
+
   return 0;
 }