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
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がおかしいとのことで,
さすがにややこしすぎたので,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); } );
これで,出来上がりかなぁ.
コメントを残す