S Morita / mbed-mros2

Porting mros2 as an Mbed library.

Dependents:   mbed-os-example-mros2 example-mbed-mros2-sub-pose example-mbed-mros2-pub-twist example-mbed-mros2-mturtle-teleop

src/mros2.cpp@7:c80f65422d99, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 09:23:37 2022 +0900
Revision:
7:c80f65422d99
Parent:
2:159877d749c2 
Merge test_assortment_of_msgs branch.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:580aba13d1a1 1 #include "mros2.h"
smoritaemb 0:580aba13d1a1 2 #include "mros2_user_config.h"
smoritaemb 0:580aba13d1a1 3
smoritaemb 0:580aba13d1a1 4 #include <rtps/rtps.h>
smoritaemb 0:580aba13d1a1 5
smoritaemb 0:580aba13d1a1 6 #ifdef __MBED__
smoritaemb 0:580aba13d1a1 7 #include "mbed.h"
smoritaemb 0:580aba13d1a1 8 #else /* __MBED__ */
smoritaemb 0:580aba13d1a1 9 #include "cmsis_os.h"
smoritaemb 0:580aba13d1a1 10 #endif /* __MBED__ */
smoritaemb 0:580aba13d1a1 11
smoritaemb 0:580aba13d1a1 12
smoritaemb 0:580aba13d1a1 13 namespace mros2
smoritaemb 0:580aba13d1a1 14 {
smoritaemb 0:580aba13d1a1 15
smoritaemb 0:580aba13d1a1 16 rtps::Domain *domain_ptr = NULL;
smoritaemb 0:580aba13d1a1 17 rtps::Participant *part_ptr = NULL; //TODO: detele this
smoritaemb 0:580aba13d1a1 18 rtps::Writer *pub_ptr = NULL;
smoritaemb 0:580aba13d1a1 19
smoritaemb 0:580aba13d1a1 20 #define SUB_MSG_SIZE 4 // addr size
smoritaemb 2:159877d749c2 21 osMessageQueueId_t subscriber_msg_queue_id;
smoritaemb 0:580aba13d1a1 22
smoritaemb 0:580aba13d1a1 23 bool completeNodeInit = false;
smoritaemb 0:580aba13d1a1 24 uint8_t endpointId = 0;
smoritaemb 0:580aba13d1a1 25 uint32_t subCbArray[10];
smoritaemb 0:580aba13d1a1 26
smoritaemb 0:580aba13d1a1 27 uint8_t buf[100];
smoritaemb 0:580aba13d1a1 28 uint8_t buf_index = 4;
smoritaemb 0:580aba13d1a1 29
smoritaemb 0:580aba13d1a1 30 /* Callback function to set the boolean to true upon a match */
smoritaemb 0:580aba13d1a1 31 void setTrue(void* args)
smoritaemb 0:580aba13d1a1 32 {
smoritaemb 0:580aba13d1a1 33 *static_cast<volatile bool*>(args) = true;
smoritaemb 0:580aba13d1a1 34 }
smoritaemb 0:580aba13d1a1 35
smoritaemb 0:580aba13d1a1 36 bool subMatched = false;
smoritaemb 0:580aba13d1a1 37 bool pubMatched = false;
smoritaemb 0:580aba13d1a1 38
smoritaemb 0:580aba13d1a1 39 void pubMatch(void* args)
smoritaemb 0:580aba13d1a1 40 {
smoritaemb 0:580aba13d1a1 41 MROS2_DEBUG("[MROS2LIB] publisher matched with remote subscriber");
smoritaemb 0:580aba13d1a1 42 }
smoritaemb 0:580aba13d1a1 43
smoritaemb 0:580aba13d1a1 44 void subMatch(void* args)
smoritaemb 0:580aba13d1a1 45 {
smoritaemb 0:580aba13d1a1 46 MROS2_DEBUG("[MROS2LIB] subscriber matched with remote publisher");
smoritaemb 0:580aba13d1a1 47 }
smoritaemb 0:580aba13d1a1 48
smoritaemb 0:580aba13d1a1 49
smoritaemb 0:580aba13d1a1 50 /*
smoritaemb 0:580aba13d1a1 51 * Initialization of mROS 2 environment
smoritaemb 0:580aba13d1a1 52 */
smoritaemb 0:580aba13d1a1 53 #ifdef __MBED__
smoritaemb 0:580aba13d1a1 54 Thread *mros2_init_thread;
smoritaemb 0:580aba13d1a1 55 #endif /* __MBED__ */
smoritaemb 0:580aba13d1a1 56 void init(int argc, char * argv[])
smoritaemb 0:580aba13d1a1 57 {
smoritaemb 0:580aba13d1a1 58 buf[0] = 0;
smoritaemb 0:580aba13d1a1 59 buf[1] = 1;
smoritaemb 0:580aba13d1a1 60 buf[2] = 0;
smoritaemb 0:580aba13d1a1 61 buf[3] = 0;
smoritaemb 0:580aba13d1a1 62
smoritaemb 0:580aba13d1a1 63 #ifdef __MBED__
smoritaemb 0:580aba13d1a1 64 mros2_init_thread = new Thread(osPriorityAboveNormal, 2000, nullptr, "mROS2Thread");
smoritaemb 0:580aba13d1a1 65 mros2_init_thread->start(callback(mros2_init, (void *)NULL));
smoritaemb 0:580aba13d1a1 66 #else /* __MBED__ */
smoritaemb 0:580aba13d1a1 67 osThreadAttr_t attributes;
smoritaemb 0:580aba13d1a1 68
smoritaemb 0:580aba13d1a1 69 attributes.name = "mROS2Thread",
smoritaemb 0:580aba13d1a1 70 attributes.stack_size = 1000,
smoritaemb 0:580aba13d1a1 71 attributes.priority = (osPriority_t)24,
smoritaemb 0:580aba13d1a1 72
smoritaemb 0:580aba13d1a1 73 osThreadNew(mros2_init, NULL, (const osThreadAttr_t*)&attributes);
smoritaemb 0:580aba13d1a1 74 #endif /* __MBED__ */
smoritaemb 0:580aba13d1a1 75
smoritaemb 0:580aba13d1a1 76 }
smoritaemb 0:580aba13d1a1 77
smoritaemb 0:580aba13d1a1 78 void mros2_init(void *args)
smoritaemb 0:580aba13d1a1 79 {
smoritaemb 0:580aba13d1a1 80 osStatus_t ret;
smoritaemb 0:580aba13d1a1 81
smoritaemb 0:580aba13d1a1 82 MROS2_DEBUG("[MROS2LIB] mros2_init task start");
smoritaemb 0:580aba13d1a1 83
smoritaemb 0:580aba13d1a1 84 #ifndef __MBED__
smoritaemb 0:580aba13d1a1 85 MX_LWIP_Init();
smoritaemb 0:580aba13d1a1 86 MROS2_DEBUG("[MROS2LIB] Initilizing lwIP complete");
smoritaemb 0:580aba13d1a1 87 #endif /* __MBED__ */
smoritaemb 0:580aba13d1a1 88
smoritaemb 0:580aba13d1a1 89 int sub_msg_count;
smoritaemb 0:580aba13d1a1 90 static rtps::Domain domain;
smoritaemb 0:580aba13d1a1 91 domain_ptr = &domain;
smoritaemb 0:580aba13d1a1 92
smoritaemb 0:580aba13d1a1 93 #ifndef __MBED__
smoritaemb 0:580aba13d1a1 94 sub_msg_count = mros2_get_submsg_count();
smoritaemb 2:159877d749c2 95 subscriber_msg_queue_id = osMessageQueueNew(sub_msg_count, SUB_MSG_SIZE, NULL);
smoritaemb 2:159877d749c2 96 if (subscriber_msg_queue_id == NULL) {
smoritaemb 0:580aba13d1a1 97 MROS2_ERROR("[MROS2LIB] ERROR: mROS2 init failed");
smoritaemb 0:580aba13d1a1 98 return;
smoritaemb 0:580aba13d1a1 99 }
smoritaemb 0:580aba13d1a1 100 #endif /* __MBED__ */
smoritaemb 0:580aba13d1a1 101
smoritaemb 0:580aba13d1a1 102 /* wait until participant(node) is created */
smoritaemb 0:580aba13d1a1 103 while(!completeNodeInit) {
smoritaemb 0:580aba13d1a1 104 osDelay(100);
smoritaemb 0:580aba13d1a1 105 }
smoritaemb 0:580aba13d1a1 106 domain.completeInit();
smoritaemb 0:580aba13d1a1 107 MROS2_DEBUG("[MROS2LIB] Initilizing Domain complete");
smoritaemb 0:580aba13d1a1 108
smoritaemb 0:580aba13d1a1 109 while(!subMatched && !pubMatched) {
smoritaemb 0:580aba13d1a1 110 osDelay(1000);
smoritaemb 0:580aba13d1a1 111 }
smoritaemb 0:580aba13d1a1 112
smoritaemb 0:580aba13d1a1 113 MROS2_DEBUG("[MROS2LIB] mros2_init task end");
smoritaemb 0:580aba13d1a1 114
smoritaemb 0:580aba13d1a1 115 ret = osThreadTerminate(NULL);
smoritaemb 0:580aba13d1a1 116 if (ret != osOK) {
smoritaemb 0:580aba13d1a1 117 MROS2_ERROR("[MROS2LIB] ERROR: mros2_init() task terminate error %d", ret);
smoritaemb 0:580aba13d1a1 118 }
smoritaemb 0:580aba13d1a1 119 }
smoritaemb 0:580aba13d1a1 120
smoritaemb 0:580aba13d1a1 121
smoritaemb 0:580aba13d1a1 122 /*
smoritaemb 0:580aba13d1a1 123 * Node functions
smoritaemb 0:580aba13d1a1 124 */
smoritaemb 0:580aba13d1a1 125 Node Node::create_node(std::string node_name)
smoritaemb 0:580aba13d1a1 126 {
smoritaemb 0:580aba13d1a1 127 MROS2_DEBUG("[MROS2LIB] create_node");
smoritaemb 0:580aba13d1a1 128 MROS2_DEBUG("[MROS2LIB] start creating participant");
smoritaemb 0:580aba13d1a1 129
smoritaemb 0:580aba13d1a1 130 while(domain_ptr == NULL) {
smoritaemb 0:580aba13d1a1 131 osDelay(100);
smoritaemb 0:580aba13d1a1 132 }
smoritaemb 0:580aba13d1a1 133
smoritaemb 0:580aba13d1a1 134 Node node;
smoritaemb 0:580aba13d1a1 135 node.part = domain_ptr->createParticipant();
smoritaemb 0:580aba13d1a1 136 /* TODO: utilize node name */
smoritaemb 0:580aba13d1a1 137 node.node_name = node_name;
smoritaemb 0:580aba13d1a1 138 part_ptr = node.part;
smoritaemb 0:580aba13d1a1 139 if(node.part == nullptr) {
smoritaemb 0:580aba13d1a1 140 MROS2_ERROR("[MROS2LIB] ERROR: create_node() failed");
smoritaemb 0:580aba13d1a1 141 while(true) {}
smoritaemb 0:580aba13d1a1 142 }
smoritaemb 0:580aba13d1a1 143 completeNodeInit = true;
smoritaemb 0:580aba13d1a1 144
smoritaemb 0:580aba13d1a1 145 MROS2_DEBUG("[MROS2LIB] successfully created participant");
smoritaemb 0:580aba13d1a1 146 return node;
smoritaemb 0:580aba13d1a1 147 }
smoritaemb 0:580aba13d1a1 148
smoritaemb 0:580aba13d1a1 149
smoritaemb 0:580aba13d1a1 150 /*
smoritaemb 0:580aba13d1a1 151 * Publisher functions
smoritaemb 0:580aba13d1a1 152 */
smoritaemb 0:580aba13d1a1 153 template <class T>
smoritaemb 0:580aba13d1a1 154 Publisher Node::create_publisher(std::string topic_name, int qos)
smoritaemb 0:580aba13d1a1 155 {
smoritaemb 0:580aba13d1a1 156 rtps::Writer* writer = domain_ptr->createWriter(*part_ptr, ("rt/"+topic_name).c_str(), message_traits::TypeName<T*>().value(), false);
smoritaemb 0:580aba13d1a1 157 if(writer == nullptr) {
smoritaemb 0:580aba13d1a1 158 MROS2_ERROR("[MROS2LIB] ERROR: failed to create writer in create_publisher()");
smoritaemb 0:580aba13d1a1 159 while(true) {}
smoritaemb 0:580aba13d1a1 160 }
smoritaemb 0:580aba13d1a1 161
smoritaemb 0:580aba13d1a1 162 Publisher pub;
smoritaemb 0:580aba13d1a1 163 pub_ptr = writer;
smoritaemb 0:580aba13d1a1 164 pub.topic_name = topic_name;
smoritaemb 0:580aba13d1a1 165
smoritaemb 0:580aba13d1a1 166 /* Register callback to ensure that a publisher is matched to the writer before sending messages */
smoritaemb 0:580aba13d1a1 167 part_ptr->registerOnNewSubscriberMatchedCallback(pubMatch, &subMatched);
smoritaemb 0:580aba13d1a1 168
smoritaemb 0:580aba13d1a1 169 MROS2_DEBUG("[MROS2LIB] create_publisher complete.");
smoritaemb 0:580aba13d1a1 170 return pub;
smoritaemb 0:580aba13d1a1 171 }
smoritaemb 0:580aba13d1a1 172
smoritaemb 0:580aba13d1a1 173 template <class T>
smoritaemb 0:580aba13d1a1 174 void Publisher::publish(T& msg)
smoritaemb 0:580aba13d1a1 175 {
smoritaemb 0:580aba13d1a1 176 msg.copyToBuf(&buf[4]);
smoritaemb 2:159877d749c2 177 msg.memAlign(&buf[4]);
smoritaemb 0:580aba13d1a1 178 pub_ptr->newChange(rtps::ChangeKind_t::ALIVE, buf, msg.getTotalSize() + 4);
smoritaemb 0:580aba13d1a1 179 }
smoritaemb 0:580aba13d1a1 180
smoritaemb 0:580aba13d1a1 181
smoritaemb 0:580aba13d1a1 182 /*
smoritaemb 0:580aba13d1a1 183 * Subscriber functions
smoritaemb 0:580aba13d1a1 184 */
smoritaemb 0:580aba13d1a1 185 typedef struct {
smoritaemb 0:580aba13d1a1 186 void (*cb_fp)(intptr_t);
smoritaemb 0:580aba13d1a1 187 intptr_t argp;
smoritaemb 0:580aba13d1a1 188 } SubscribeDataType;
smoritaemb 0:580aba13d1a1 189
smoritaemb 0:580aba13d1a1 190 template <class T>
smoritaemb 0:580aba13d1a1 191 Subscriber Node::create_subscription(std::string topic_name, int qos, void(*fp)(T*))
smoritaemb 0:580aba13d1a1 192 {
smoritaemb 0:580aba13d1a1 193 rtps::Reader* reader = domain_ptr->createReader(*(this->part), ("rt/"+topic_name).c_str(), message_traits::TypeName<T*>().value(), false);
smoritaemb 0:580aba13d1a1 194 if(reader == nullptr) {
smoritaemb 0:580aba13d1a1 195 MROS2_ERROR("[MROS2LIB] ERROR: failed to create reader in create_subscription()");
smoritaemb 0:580aba13d1a1 196 while(true) {}
smoritaemb 0:580aba13d1a1 197 }
smoritaemb 0:580aba13d1a1 198
smoritaemb 0:580aba13d1a1 199 Subscriber sub;
smoritaemb 0:580aba13d1a1 200 sub.topic_name = topic_name;
smoritaemb 0:580aba13d1a1 201 sub.cb_fp = (void (*)(intptr_t))fp;
smoritaemb 0:580aba13d1a1 202
smoritaemb 0:580aba13d1a1 203 SubscribeDataType *data_p;
smoritaemb 0:580aba13d1a1 204 data_p = new SubscribeDataType;
smoritaemb 0:580aba13d1a1 205 data_p->cb_fp = (void (*)(intptr_t))fp;
smoritaemb 0:580aba13d1a1 206 data_p->argp = (intptr_t)NULL;
smoritaemb 0:580aba13d1a1 207 reader->registerCallback(sub.callback_handler<T>, (void *)data_p);
smoritaemb 0:580aba13d1a1 208
smoritaemb 0:580aba13d1a1 209 /* Register callback to ensure that a subscriber is matched to the reader before receiving messages */
smoritaemb 0:580aba13d1a1 210 part_ptr->registerOnNewPublisherMatchedCallback(subMatch, &pubMatched);
smoritaemb 0:580aba13d1a1 211
smoritaemb 0:580aba13d1a1 212 MROS2_DEBUG("[MROS2LIB] create_subscription complete. data memory address=0x%x", data_p);
smoritaemb 0:580aba13d1a1 213 return sub;
smoritaemb 0:580aba13d1a1 214 }
smoritaemb 0:580aba13d1a1 215
smoritaemb 0:580aba13d1a1 216 template <class T>
smoritaemb 0:580aba13d1a1 217 void Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange)
smoritaemb 0:580aba13d1a1 218 {
smoritaemb 0:580aba13d1a1 219 T msg;
smoritaemb 2:159877d749c2 220 msg.copyFromBuf(&cacheChange.data[4]);
smoritaemb 0:580aba13d1a1 221
smoritaemb 0:580aba13d1a1 222 SubscribeDataType *sub = (SubscribeDataType *)callee;
smoritaemb 0:580aba13d1a1 223 void (*fp)(intptr_t) = sub->cb_fp;
smoritaemb 0:580aba13d1a1 224 fp((intptr_t)&msg);
smoritaemb 0:580aba13d1a1 225 }
smoritaemb 0:580aba13d1a1 226
smoritaemb 0:580aba13d1a1 227
smoritaemb 0:580aba13d1a1 228 /*
smoritaemb 0:580aba13d1a1 229 * Other utility functions
smoritaemb 0:580aba13d1a1 230 */
smoritaemb 0:580aba13d1a1 231 void spin()
smoritaemb 0:580aba13d1a1 232 {
smoritaemb 0:580aba13d1a1 233 while(true) {
smoritaemb 0:580aba13d1a1 234 #ifndef __MBED__
smoritaemb 0:580aba13d1a1 235 osStatus_t ret;
smoritaemb 0:580aba13d1a1 236 SubscribeDataType* msg;
smoritaemb 2:159877d749c2 237 ret = osMessageQueueGet(subscriber_msg_queue_id, &msg, NULL, osWaitForever);
smoritaemb 0:580aba13d1a1 238 if (ret != osOK) {
smoritaemb 0:580aba13d1a1 239 MROS2_ERROR("[MROS2LIB] ERROR: mROS2 spin() wait error %d", ret);
smoritaemb 0:580aba13d1a1 240 }
smoritaemb 0:580aba13d1a1 241 #else /* __MBED__ */
smoritaemb 0:580aba13d1a1 242 // The queue above seems not to be pushed anywhere. So just sleep.
smoritaemb 0:580aba13d1a1 243 ThisThread::sleep_for(1000);
smoritaemb 0:580aba13d1a1 244 #endif /* __MBED__ */
smoritaemb 0:580aba13d1a1 245 }
smoritaemb 0:580aba13d1a1 246 }
smoritaemb 0:580aba13d1a1 247
smoritaemb 0:580aba13d1a1 248 } /* namespace mros2 */
smoritaemb 0:580aba13d1a1 249
smoritaemb 0:580aba13d1a1 250
smoritaemb 0:580aba13d1a1 251 /*
smoritaemb 0:580aba13d1a1 252 * Declaration for embeddedRTPS participants
smoritaemb 0:580aba13d1a1 253 */
smoritaemb 0:580aba13d1a1 254 void *networkSubDriverPtr;
smoritaemb 0:580aba13d1a1 255 void *networkPubDriverPtr;
smoritaemb 0:580aba13d1a1 256 void (*hbPubFuncPtr)(void *);
smoritaemb 0:580aba13d1a1 257 void (*hbSubFuncPtr)(void *);
smoritaemb 0:580aba13d1a1 258
smoritaemb 0:580aba13d1a1 259 extern "C" void callHbPubFunc(void *arg)
smoritaemb 0:580aba13d1a1 260 {
smoritaemb 0:580aba13d1a1 261 if(hbPubFuncPtr != NULL && networkPubDriverPtr != NULL) {
smoritaemb 0:580aba13d1a1 262 (*hbPubFuncPtr)(networkPubDriverPtr);
smoritaemb 0:580aba13d1a1 263 }
smoritaemb 0:580aba13d1a1 264 }
smoritaemb 0:580aba13d1a1 265 extern "C" void callHbSubFunc(void *arg)
smoritaemb 0:580aba13d1a1 266 {
smoritaemb 0:580aba13d1a1 267 if(hbSubFuncPtr != NULL && networkSubDriverPtr != NULL) {
smoritaemb 0:580aba13d1a1 268 (*hbSubFuncPtr)(networkSubDriverPtr);
smoritaemb 0:580aba13d1a1 269 }
smoritaemb 0:580aba13d1a1 270 }
smoritaemb 0:580aba13d1a1 271
smoritaemb 0:580aba13d1a1 272 void setTrue(void* args)
smoritaemb 0:580aba13d1a1 273 {
smoritaemb 0:580aba13d1a1 274 *static_cast<volatile bool*>(args) = true;
smoritaemb 0:580aba13d1a1 275 }
smoritaemb 0:580aba13d1a1 276
smoritaemb 0:580aba13d1a1 277 /*
smoritaemb 2:159877d749c2 278 * specialize template functions described in platform's workspace
smoritaemb 0:580aba13d1a1 279 */
smoritaemb 2:159877d749c2 280 #include "templates.hpp"