ros2-rcll_refbox_peer を作ってみる(その2)

ros2-rcll_refbox_peer を作ってみる.でようやく準備ができしたので,
ros2-rcll_refbox_peer.cpp を触っていきます.

まず,ros::Publisher を単純にrclcpp::Publisherに置換したけど,

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:98:19: error: class template argument deduction failed:
   98 | rclcpp::Publisher pub_beacon_;
      |                   ^~~~~~~~~~~

とエラーが出た.
どうやら,rclcpp::Publisher::SharedPtr といった感じでメッセージの形を指定する必要があるみたい.

rclcpp::Publisher<refbox_msgs::msg::BeaconSignal>::SharedPtr pub_beacon_;
rclcpp::Publisher<refbox_msgs::msg::GameState>::SharedPtr pub_game_state_;
rclcpp::Publisher<refbox_msgs::msg::MachineInfo>::SharedPtr pub_machine_info_;
rclcpp::Publisher<refbox_msgs::msg::ExplorationInfo>::SharedPtr pub_exploration_info_;
rclcpp::Publisher<refbox_msgs::msg::MachineReportInfo>::SharedPtr pub_machine_report_info_;
rclcpp::Publisher<refbox_msgs::msg::OrderInfo>::SharedPtr pub_order_info_;
rclcpp::Publisher<refbox_msgs::msg::RingInfo>::SharedPtr pub_ring_info_;
rclcpp::Publisher<refbox_msgs::msg::NavigationRoutes>::SharedPtr pub_navigation_routes_info_;
rclcpp::Publisher<refbox_msgs::msg::AgentTask>::SharedPtr pub_agent_task_;

rclcpp::Service<refbox_msgs::srv::SendBeaconSignal>::SharedPtr srv_send_beacon_;
rclcpp::Service<refbox_msgs::srv::SendMachineReportBTR>::SharedPtr srv_send_machine_report_;
rclcpp::Service<refbox_msgs::srv::SendPrepareMachine>::SharedPtr srv_send_prepare_machine_;
rclcpp::Service<refbox_msgs::srv::SendAgentTask>::SharedPtr srv_send_agent_task_;

次は,メッセージ出力のマクロ(ROS_INFOとかROS_WARNとか)の引数が変わったみたい.

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp: In function ‘void handle_recv_error(boost::asio::ip::udp::endpoint&, std::string)’:
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:126:3: error: static assertion failed: First argument to logging macros must be an rclcpp::Logger
  126 |   RCLCPP_WARN("Receive error from %s:%u: %s",

第一引数は,rclcpp::Logger になっているみたいなので,rclcpp::get_logger(“rclcpp”) って書き方になるみたい.

RCLCPP_WARN(rclcpp::get_logger("rclcpp"), <元々の引数...>)

って感じ.

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:141:42: error: ‘refbox_msgs::Team’ has not been declared
  141 |                 return (int)refbox_msgs::Team::CYAN;

次は,なんだ….refbox_msgs::msg::Team::CYAN ですね.
ROS2になって,msgが加わったのを忘れていました.
他も同様….

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:214:41: error: ‘using SharedPtr = class std::shared_ptr<rclcpp::Publisher<refbox_msgs::msg::AgentTask_<std::allocator<void> > > >’ {aka ‘class std::shared_ptr<rclcpp::Publisher<refbox_msgs::msg::AgentTask_<std::allocator<void> > > >’} has no member named ‘publish’
  214 |                         pub_agent_task_.publish(rat);
      |   

あー.pub_agent_task_がshared_ptrにしちゃったから,ポインタ(->)で書かないといけませんね.
他も同様なり.

pub_agent_task_->publish(rat);
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:224:35: error: ‘using _header_type = struct std_msgs::msg::Header_<std::allocator<void> >’ {aka ‘struct std_msgs::msg::Header_<std::allocator<void> >’} has no member named ‘seq’
  224 |                         rb.header.seq = b->seq();

えとー.Headerを見てみると,seqがなくなっているっぽいですね.
とりあえず,ここに関してはコメントアウトで様子見.

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp: In function ‘void handle_message(boost::asio::ip::udp::endpoint&, uint16_t, uint16_t, std::shared_ptr<google::protobuf::Message>)’:
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:400:49: warning: unused variable ‘rz’ [-Wunused-variable]
  400 |                                 llsf_msgs::Zone rz;
      |    

知ったこっちゃねー って言いたいところ.
warning だから,とりあえず無視しておきます.

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:549:53: error: ‘using _stamp_type = struct builtin_interfaces::msg::Time_<std::allocator<void> >’ {aka ‘struct builtin_interfaces::msg::Time_<std::allocator<void> >’} has no member named ‘nsec’; did you mean ‘sec’?
  549 |         b.mutable_time()->set_nsec(req.header.stamp.nsec);
      |                                                     ^~~~
      |                                                     sec
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:550:30: error: ‘using _header_type = struct std_msgs::msg::Header_<std::allocator<void> >’ {aka ‘struct std_msgs::msg::Header_<std::allocator<void> >’} has no member named ‘seq’
  550 |         b.set_seq(req.header.seq);
      |                              ^~~
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:566:95: error: ‘using _stamp_type = struct builtin_interfaces::msg::Time_<std::allocator<void> >’ {aka ‘struct builtin_interfaces::msg::Time_<std::allocator<void> >’} has no member named ‘nsec’; did you mean ‘sec’?
  566 |                         b.mutable_pose()->mutable_timestamp()->set_nsec(req.pose.header.stamp.nsec);
      |                                                                                               ^~~~
      |  

timestampのところですね.
builtin_interfaces/Timeを見てみると,

int32 sec
uint32 nanosec

とのことなので,nsecをnanosecに書き換え(関数の方は,set_nsecのまま….ズレがありますねぇ…).
seqは,存在しなかったので,0にしておきましょうか(これも様子見).

        b.mutable_time()->set_nsec(req.header.stamp.nanosec);
        // b.set_seq(req.header.seq);
        b.set_seq(0);
...
                        b.mutable_pose()->mutable_timestamp()->set_nsec(req.pose.header.stamp.nsec);
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:813:34: error: invalid initialization of reference of type ‘const rclcpp::InitOptions&’ from expression of type ‘const char [17]’
  813 |         rclcpp::init(argc, argv, "rcll_refbox_peer");
      |                                  ^~~~~~~~~~~~~~~~~~

名前はNodeを作る時に指定する形になったっぽい.

rclcpp::init(argc, argv);
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:815:17: error: ‘NodeHandle’ is not a member of ‘rclcpp’
  815 |         rclcpp::NodeHandle n;
      |     

で,こちらで名前を指定しないといけない.

rclcpp::Node::SharedPtr n = nullptr;
n = rclcpp::Node::make_shared("rcll_refbox_perr");
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:819:77: error: ‘rclcpp::this_node’ has not been declared
  819 |         RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "%s starting up", rclcpp::this_node::getName().c_str());
      |  

んとー.this_nodeの代わりにNodeになって,get_name()ってのに変わります.

        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "%s starting up", n->get_name());
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:70:31: error: ‘rclcpp::param’ has not been declared
   70 |                 if (! rclcpp::param::get("~" #P, cfg_ ## P ## _)) { \
      |    

あ,マクロの部分か.戻り値によって,.as_string()とか,.as_int()とか変える必要があるっぽい.

#define GET_PRIV_PARAM_STRING(P)         \
        { \
                cfg_ ## P ## _ = n->get_parameter("~" #P).as_string(); \
                if ( "" == cfg_ ## P ## _) { \
                        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to retrieve parameter " #P ", aborting"); \
                        exit(-1); \
                } \
        }

#define GET_PRIV_PARAM_INT(P)         \
        { \
                cfg_ ## P ## _ = n->get_parameter("~" #P).as_int(); \
                if ( 0 == cfg_ ## P ## _) { \
                        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to retrieve parameter " #P ", aborting"); \
                        exit(-1); \
                } \
        }

あと,ROS2でパラメータを扱うには,先に宣言しておく必要があるっぽい.

       // declear_parameter
        n->declare_parameter<std::string>("~team_name");
        n->declare_parameter<std::string>("~robot_name");
        n->declare_parameter<std::string>("~robot_number");
        n->declare_parameter<std::string>("~peer_address");
        n->declare_parameter<std::string>("~peer_public_recv_port");
        n->declare_parameter<std::string>("~peer_public_send_port");
        n->declare_parameter<std::string>("~peer_public_port");
        n->declare_parameter<std::string>("~peer_cyan_recv_port");
        n->declare_parameter<std::string>("~peer_cyan_send_port");
        n->declare_parameter<std::string>("~peer_cyan_port");
        n->declare_parameter<std::string>("~peer_magenta_recv_port");
        n->declare_parameter<std::string>("~peer_magenta_send_port");
        n->declare_parameter<std::string>("~peer_magenta_port");
        n->declare_parameter<std::string>("~crypto_key");
        n->declare_parameter<std::string>("~crypto_cipher");

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:878:29: error: ‘using SharedPtr = class std::shared_ptr<rclcpp::Node>’ {aka ‘class std::shared_ptr<rclcpp::Node>’} has no member named ‘advertise’
  878 |         pub_agent_task_ = n.advertise<refbox_msgs::msg::AgentTask>("rcll/agent_task", 100);
      |  

あ,nがポインタになっているから,「.」ではなく「->」ですね.
あと,advertiseが無くなっていて,create_publisherに変わっています.

/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:838:21: error: ‘rclcpp::para’ has not been declared
  838 |         if (rclcpp::param::has("~peer_public_recv_port") && rclcpp::param::has("~peer_public_send_port")) {
      |    

あぁ.ここもパラメーター関係か….
困ったなぁ….仕方がない.一回変数に代入するか.
(本当は,値がない時の処理をちゃんと書かないといけない気がするけど,後回し)

        int peer_public_recv_port_   = n->get_parameter("~peer_public_recv_port").as_int();
        int peer_public_send_port_   = n->get_parameter("~peer_public_send_port").as_int();
        if (peer_public_recv_port_ != 0 && peer_public_send_port_ != 0) {
                cfg_peer_public_local_ = true;
                GET_PRIV_PARAM_INT(peer_public_recv_port);
                GET_PRIV_PARAM_INT(peer_public_send_port);
        } else {
                cfg_peer_public_local_ = false;
                GET_PRIV_PARAM_INT(peer_public_port);
        }
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:974:28: error: ‘using SharedPtr = class std::shared_ptr<rclcpp::Node>’ {aka ‘class std::shared_ptr<rclcpp::Node>’} has no member named ‘advertiseService’
  974 |   srv_send_agent_task_ = n.advertiseService("/rcll/send_agent_task", srv_cb_send_agent_task);
      |      

今度は,serviceの書き方の部分か….さっきのcreate_publisherと一緒ですね.

  // provide services
  srv_send_agent_task_ = n->create_service<refbox_msgs::srv::SendAgentTask>("/rcll/send_agent_task", &srv_cb_send_agent_task);
  srv_send_beacon_ = n->create_service<refbox_msgs::srv::SendBeaconSignal>("rcll/send_beacon", &srv_cb_send_beacon);
  srv_send_machine_report_ = n->create_service<refbox_msgs::srv::SendMachineReportBTR>("rcll/send_machine_report", &srv_cb_send_machine_report);
  srv_send_prepare_machine_ = n->create_service<refbox_msgs::srv::SendPrepareMachine>("rcll/send_prepare_machine", &srv_cb_send_prepare_machine);
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:979:15: error: no matching function for call to ‘spin()’
  979 |   rclcpp::spin();

最後かなぁ.spinの部分は,spin(n);にするだけ.

rclcpp::spin(n);
/home/ryukoku/ros2_ws/src/ros2-rcll_refbox_peer/refbox_peer/src/ros2-rcll_refbox_peer.cpp:974:75:   required from here
/opt/ros/humble/include/rclcpp/rclcpp/any_service_callback.hpp:148:17: error: no match for ‘operator=’ (operand types are ‘std::variant<std::monostate, std::function<void(std::shared_ptr<refbox_msgs::srv::SendBeaconSignal_Request_<std::allocator<void> > >, std::shared_ptr<refbox_msgs::srv::SendBeaconSignal_Response_<std::allocator<void> > >)>, std::function<void(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<refbox_msgs::srv::SendBeaconSignal_Request_<std::allocator<void> > >, std::shared_ptr<refbox_msgs::srv::SendBeaconSignal_Response_<std::allocator<void> > >)>, std::function<void(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<refbox_msgs::srv::SendBeaconSignal_Request_<std::allocator<void> > >)>, std::function<void(std::shared_ptr<rclcpp::Service<refbox_msgs::srv::SendBeaconSignal> >, std::shared_ptr<rmw_request_id_s>, std::shared_ptr<refbox_msgs::srv::SendBeaconSignal_Request_<std::allocator<void> > >)> >’ and ‘bool (*)(refbox_msgs::srv::SendBeaconSignal_Request_<std::allocator<void> >&, refbox_msgs::srv::SendBeaconSignal_Response_<std::allocator<void> >&)’)
  148 |       callback_ = std::forward<CallbackT>(callback);

まだエラーが出てきた….
Sendのサービスのcallbackがおかしいとのことで,Writing a simple service and client (C++)と見比べながら修正.
さすがにややこしすぎたので,chatGPTさんに助けてもらいました.

エラーの原因は、ROS2のサービスのコールバック関数のシグネチャが正しくないためです。
エラーメッセージのポイントを整理すると:

callback_ には std::variant 型のコールバックが設定されるが、渡された関数の型 (void (*)(Request&, Response&)) が std::variant のいずれの型とも一致しない。
std::variant に含まれる型を見ると、コールバック関数は通常 std::shared_ptr を引数に取るべきである。

流石ですね.
修正案も出してくれました.

void service_callback(refbox_msgs::srv::SendAgentTask_Request& request,
                      refbox_msgs::srv::SendAgentTask_Response& response) {
    // 処理を書く
}
    // サービスの登録(ラムダ関数を使用)
    auto service = node->create_service<refbox_msgs::srv::SendAgentTask>(
        "send_agent_task",
        [](const std::shared_ptr<refbox_msgs::srv::SendAgentTask_Request> request,
           std::shared_ptr<refbox_msgs::srv::SendAgentTask_Response> response) {
            service_callback(request, response);
        }
    );

これで,出来上がりかなぁ.

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です

*