mirs_esp
読み取り中…
検索中…
一致する文字列を見つけられません
ros_setup.ino
[詳解]
1void ros_setup(){
2 //micro-rosのセッティング
3 set_microros_transports();
4 delay(500);
5
6 allocator = rcl_get_default_allocator();
7
8 //nodeの作成とros_domein_idの作成
9 //rosid_setup_foxy();
11
12 //サブスクライバ、パブリッシャー、サービスの宣言
13 rclc_publisher_init_default(
14 &enc_pub,
15 &node,
16 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
17 "/encoder"
18 );
19
20 rclc_publisher_init_default(
21 &vlt_pub,
22 &node,
23 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
24 "/vlt"
25 );
26
27 rclc_publisher_init_default(
29 &node,
30 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
31 "/vel"
32 );
33
34 rclc_subscription_init_default(
36 &node,
37 ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
38 "/cmd_vel"
39 );
40
41 rclc_subscription_init_default(
42 &param_sub,
43 &node,
44 ROSIDL_GET_MSG_TYPE_SUPPORT(mirs_msgs, msg, BasicParam),
45 "/params"
46 );
47
48 rclc_service_init_default(
50 &node,
51 ROSIDL_GET_SRV_TYPE_SUPPORT(mirs_msgs, srv, ParameterUpdate),
52 "/esp_update"
53 );
54
55 rclc_service_init_default(
56 &reset_srv,
57 &node,
58 ROSIDL_GET_SRV_TYPE_SUPPORT(mirs_msgs, srv, SimpleCommand),
59 "/reset_encoder"
60 );
61
62 const uint32_t timer_timeout = 100;
63
64 rclc_timer_init_default(
65 &timer,
66 &support,
67 RCL_MS_TO_NS(timer_timeout),
69 );
70
71 //イベント発生の設定(数字はイベントの発生点の数)
72 //デフォルトの発生点はsubscriberが2(/cmd_vel,/params)、serviceが2(/reset,/update)、timerが1(定期実行)の合計5
73 rclc_executor_init(&executor, &support.context, 5, &allocator);
74 rclc_executor_add_subscription(&executor, &cmd_vel_sub, &cmd_vel_msg, &cmd_vel_Callback, ON_NEW_DATA);
75 rclc_executor_add_subscription(&executor, &param_sub, &param_msg, &param_Callback, ON_NEW_DATA);
76 rclc_executor_add_service(&executor, &update_srv, &update_req, &update_res, update_service_callback);
77 rclc_executor_add_service(&executor, &reset_srv, &reset_req, &reset_res, reset_service_callback);
78 rclc_executor_add_timer(&executor, &timer);
79}
80
81/* ROS_DOMAIN_ID 設定用 */
82/* foxy以前の場合はrosid_setup_foxy() */
83/* humble以降の場合はrosid_setup_humble() */
84/* 多分コンパイラの関係。このプログラムの作成者のコンパイル環境はhumbleなので基本的にはrosid_setup_humbleの仕様を推奨 */
85/* ライブラリを再コンパイルする際は環境に合わせて設定するように */
86
87// void rosid_setup_foxy(){
88// rcl_node_options_t node_ops;
89// node_ops = rcl_node_get_default_options();
90// //node_ops.domain_id = ROS_DOMAIN_ID;
91// rclc_support_init(&support, 0, NULL, &allocator);
92// rclc_node_init_with_options(&node, "ESP32_node", "", &support, &node_ops);
93// }
94
96 rcl_init_options_t init_options;
97 init_options = rcl_get_zero_initialized_init_options();
98 size_t domain_id = ROS_DOMAIN_ID;
99 rcl_init_options_init(&init_options, allocator); // <--- This was missing on ur side
100 // Set ROS domain id
101 rcl_init_options_set_domain_id(&init_options, domain_id);
102 // Setup support structure.
103 rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
104 // create node
105 rclc_node_init_default(&node, "ESP32_node", "", &support);
106}
#define ROS_DOMAIN_ID
rcl_service_t reset_srv
mirs_msgs__srv__SimpleCommand_Response reset_res
rcl_publisher_t vlt_pub
rcl_publisher_t enc_pub
rcl_subscription_t param_sub
mirs_msgs__srv__SimpleCommand_Request reset_req
mirs_msgs__srv__ParameterUpdate_Response update_res
rcl_service_t update_srv
rclc_executor_t executor
mirs_msgs__msg__BasicParam param_msg
geometry_msgs__msg__Twist cmd_vel_msg
rcl_subscription_t cmd_vel_sub
mirs_msgs__srv__ParameterUpdate_Request update_req
rcl_node_t node
rcl_allocator_t allocator
rclc_support_t support
rcl_timer_t timer
rcl_publisher_t curr_vel_pub
void ros_setup()
void rosid_setup_humble()
void update_service_callback(const void *req, void *res)
void reset_service_callback(const void *req, void *res)
void param_Callback(const void *msgin)
void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
void cmd_vel_Callback(const void *msgin)