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);
}
);
これで，出来上がりかなぁ．
コメントを残す