void FeederTimerTask::run()
{
Poco::Logger::root().information("Dispense Food on aquarium");
Feeder feeder(gpio_);
try {
feeder.dispense();
if (onFeed_) {
onFeed_();
}
} catch (const std::runtime_error& error) {
Poco::Logger::root().error(error.what());
}
}
/**
* Escort Robot instantiation that will take the resident to the dining area and feed them food.
*/
int main(int argc, char **argv) {
geometry_msgs::Point base;
base.x = FEEDER_BASE_X;
base.y = FEEDER_BASE_Y;
geometry_msgs::Point table;
table.x = ADJACENT_TABLE_X;
table.y = ADJACENT_TABLE_Y;
EscortRobot feeder(EVENT_TRIGGER_EVENT_TYPE_EAT, base, table);
const std::string rid = "robot_9";
ros::init(argc, argv, "Feeding_Robot");
// Node handle
ros::NodeHandle codeHandle;
// Will publish geometry_msgs::Twist messages to the cmd_vel topic
feeder.robotNodeStagePub = codeHandle.advertise<geometry_msgs::Twist>(rid + "/cmd_vel", 1000);
feeder.eventTriggerPub = codeHandle.advertise<elderly_care_simulation::EventTrigger>("event_trigger", 1000, true);
// Necessary subscribers
feeder.stageOdoSub = codeHandle.subscribe<nav_msgs::Odometry>(rid + "/base_pose_ground_truth", 1000, &Robot::stage0domCallback, dynamic_cast<Robot*>( &feeder ));
feeder.eventTriggerSub = codeHandle.subscribe<elderly_care_simulation::EventTrigger>("event_trigger", 1000, &EscortRobot::eventTriggered, &feeder);
feeder.residentLocationSub = codeHandle.subscribe<nav_msgs::Odometry>("robot_0/base_pose_ground_truth", 1000, &EscortRobot::residentLocationCallback, &feeder);
// Service used to find paths
feeder.pathFinderService = codeHandle.serviceClient<elderly_care_simulation::FindPath>("find_path");
// Service to perform tasks on the resident
feeder.performTaskClient = codeHandle.serviceClient<elderly_care_simulation::PerformTask>("perform_task");
return feeder.execute();
}
int main() {
int rc;
int val;
/* Non-blocking receiver case. */
int ch1 = chmake(sizeof(int));
errno_assert(ch1 >= 0);
int hndl1 = go(sender1(ch1, 555));
errno_assert(hndl1 >= 0);
struct chclause cls1[] = {{CHRECV, ch1, &val, sizeof(val)}};
rc = choose(cls1, 1, -1);
choose_assert(0, 0);
assert(val == 555);
hclose(ch1);
rc = hclose(hndl1);
errno_assert(rc == 0);
/* Blocking receiver case. */
int ch2 = chmake(sizeof(int));
errno_assert(ch2 >= 0);
int hndl2 = go(sender2(ch2, 666));
errno_assert(hndl2 >= 0);
struct chclause cls2[] = {{CHRECV, ch2, &val, sizeof(val)}};
rc = choose(cls2, 1, -1);
choose_assert(0, 0);
assert(val == 666);
hclose(ch2);
rc = hclose(hndl2);
errno_assert(rc == 0);
/* Non-blocking sender case. */
int ch3 = chmake(sizeof(int));
errno_assert(ch3 >= 0);
int hndl3 = go(receiver1(ch3, 777));
errno_assert(hndl3 >= 0);
val = 777;
struct chclause cls3[] = {{CHSEND, ch3, &val, sizeof(val)}};
rc = choose(cls3, 1, -1);
choose_assert(0, 0);
hclose(ch3);
rc = hclose(hndl3);
errno_assert(rc == 0);
/* Blocking sender case. */
int ch4 = chmake(sizeof(int));
errno_assert(ch4 >= 0);
int hndl4 = go(receiver2(ch4, 888));
errno_assert(hndl4 >= 0);
val = 888;
struct chclause cls4[] = {{CHSEND, ch4, &val, sizeof(val)}};
rc = choose(cls4, 1, -1);
choose_assert(0, 0);
hclose(ch4);
rc = hclose(hndl4);
errno_assert(rc == 0);
/* Check with two channels. */
int hndl5[2];
int ch5 = chmake(sizeof(int));
errno_assert(ch5 >= 0);
int ch6 = chmake(sizeof(int));
errno_assert(ch6 >= 0);
hndl5[0] = go(sender1(ch6, 555));
errno_assert(hndl5 >= 0);
struct chclause cls5[] = {
{CHRECV, ch5, &val, sizeof(val)},
{CHRECV, ch6, &val, sizeof(val)}
};
rc = choose(cls5, 2, -1);
choose_assert(1, 0);
assert(val == 555);
hndl5[1] = go(sender2(ch5, 666));
errno_assert(hndl5 >= 0);
rc = choose(cls5, 2, -1);
choose_assert(0, 0);
assert(val == 666);
hclose(ch5);
hclose(ch6);
rc = hclose(hndl5[0]);
errno_assert(rc == 0);
rc = hclose(hndl5[1]);
errno_assert(rc == 0);
/* Test whether selection of in channels is random. */
int ch7 = chmake(sizeof(int));
errno_assert(ch7 >= 0);
int ch8 = chmake(sizeof(int));
errno_assert(ch8 >= 0);
int hndl6[2];
hndl6[0] = go(feeder(ch7, 111));
errno_assert(hndl6[0] >= 0);
hndl6[1] = go(feeder(ch8, 222));
errno_assert(hndl6[1] >= 0);
int i;
int first = 0;
int second = 0;
int third = 0;
for(i = 0; i != 100; ++i) {
struct chclause cls6[] = {
{CHRECV, ch7, &val, sizeof(val)},
{CHRECV, ch8, &val, sizeof(val)}
};
rc = choose(cls6, 2, -1);
errno_assert(rc == 0 || rc == 1);
if(rc == 0) {
assert(val == 111);
++first;
}
if(rc == 1) {
assert(val == 222);
++second;
}
int rc = yield();
errno_assert(rc == 0);
}
assert(first > 1 && second > 1);
hclose(hndl6[0]);
hclose(hndl6[1]);
hclose(ch7);
hclose(ch8);
/* Test 'otherwise' clause. */
int ch9 = chmake(sizeof(int));
errno_assert(ch9 >= 0);
struct chclause cls7[] = {{CHRECV, ch9, &val, sizeof(val)}};
rc = choose(cls7, 1, 0);
choose_assert(-1, ETIMEDOUT);
hclose(ch9);
rc = choose(NULL, 0, 0);
choose_assert(-1, ETIMEDOUT);
/* Test two simultaneous senders vs. choose statement. */
int ch10 = chmake(sizeof(int));
errno_assert(ch10 >= 0);
int hndl7[2];
hndl7[0] = go(sender1(ch10, 888));
errno_assert(hndl7[0] >= 0);
hndl7[1] = go(sender1(ch10, 999));
errno_assert(hndl7[1] >= 0);
val = 0;
struct chclause cls8[] = {{CHRECV, ch10, &val, sizeof(val)}};
rc = choose(cls8, 1, -1);
choose_assert(0, 0);
assert(val == 888);
val = 0;
rc = choose(cls8, 1, -1);
choose_assert(0, 0);
assert(val == 999);
hclose(ch10);
rc = hclose(hndl7[0]);
errno_assert(rc == 0);
rc = hclose(hndl7[1]);
errno_assert(rc == 0);
/* Test two simultaneous receivers vs. choose statement. */
int ch11 = chmake(sizeof(int));
errno_assert(ch11 >= 0);
int hndl8[2];
hndl8[0] = go(receiver1(ch11, 333));
errno_assert(hndl8[0] >= 0);
hndl8[1] = go(receiver1(ch11, 444));
errno_assert(hndl8[1] >= 0);
val = 333;
struct chclause cls9[] = {{CHSEND, ch11, &val, sizeof(val)}};
rc = choose(cls9, 1, -1);
choose_assert(0, 0);
val = 444;
rc = choose(cls9, 1, -1);
choose_assert(0, 0);
hclose(ch11);
rc = hclose(hndl8[0]);
errno_assert(rc == 0);
rc = hclose(hndl8[1]);
errno_assert(rc == 0);
/* Choose vs. choose. */
int ch12 = chmake(sizeof(int));
errno_assert(ch12 >= 0);
int hndl9 = go(choosesender(ch12, 111));
errno_assert(hndl9 >= 0);
struct chclause cls10[] = {{CHRECV, ch12, &val, sizeof(val)}};
rc = choose(cls10, 1, -1);
choose_assert(0, 0);
assert(val == 111);
hclose(ch12);
rc = hclose(hndl9);
errno_assert(rc == 0);
/* Test transferring a large object. */
int ch17 = chmake(sizeof(struct large));
errno_assert(ch17 >= 0);
int hndl10 = go(sender4(ch17));
errno_assert(hndl9 >= 0);
struct large lrg;
struct chclause cls14[] = {{CHRECV, ch17, &lrg, sizeof(lrg)}};
rc = choose(cls14, 1, -1);
choose_assert(0, 0);
hclose(ch17);
/* Test that 'in' on done-with channel fires. */
int ch18 = chmake(sizeof(int));
errno_assert(ch18 >= 0);
rc = hdone(ch18);
errno_assert(rc == 0);
struct chclause cls15[] = {{CHRECV, ch18, &val, sizeof(val)}};
rc = choose(cls15, 1, -1);
choose_assert(0, EPIPE);
hclose(ch18);
/* Test expiration of 'deadline' clause. */
int ch21 = chmake(sizeof(int));
errno_assert(ch21 >= 0);
int64_t start = now();
struct chclause cls17[] = {{CHRECV, ch21, &val, sizeof(val)}};
rc = choose(cls17, 1, start + 50);
choose_assert(-1, ETIMEDOUT);
int64_t diff = now() - start;
time_assert(diff, 50);
hclose(ch21);
/* Test unexpired 'deadline' clause. */
int ch22 = chmake(sizeof(int));
errno_assert(ch22 >= 0);
start = now();
int hndl11 = go(sender3(ch22, 4444, start + 50));
errno_assert(hndl11 >= 0);
struct chclause cls18[] = {{CHRECV, ch22, &val, sizeof(val)}};
rc = choose(cls17, 1, start + 1000);
choose_assert(0, 0);
assert(val == 4444);
diff = now() - start;
time_assert(diff, 50);
hclose(ch22);
rc = hclose(hndl11);
errno_assert(rc == 0);
/* Test that first channel in the array is prioritized. */
int ch23 = chmake(sizeof(int));
errno_assert(ch23 >= 0);
int ch24 = chmake(sizeof(int));
errno_assert(ch24 >= 0);
int hndl12 = go(sender1(ch23, 0));
errno_assert(hndl12 >= 0);
int hndl13 = go(sender1(ch24, 0));
errno_assert(hndl13 >= 0);
struct chclause cls19[] = {
{CHRECV, ch24, &val, sizeof(val)},
{CHRECV, ch23, &val, sizeof(val)}
};
rc = choose(cls19, 2, -1);
choose_assert(0, 0);
rc = chrecv(ch23, &val, sizeof(val), -1);
errno_assert(rc == 0);
rc = hclose(hndl13);
errno_assert(rc == 0);
rc = hclose(hndl12);
errno_assert(rc == 0);
rc = hclose(ch24);
errno_assert(rc == 0);
rc = hclose(ch23);
errno_assert(rc == 0);
/* Try adding the same channel to choose twice. Doing so is pointless,
but it shouldn't crash the application. */
int ch25 = chmake(sizeof(int));
errno_assert(ch25 >= 0);
struct chclause cls20[] = {
{CHRECV, ch25, &val, sizeof(val)},
{CHRECV, ch25, &val, sizeof(val)}
};
rc = choose(cls20, 2, now() + 50);
choose_assert(-1, ETIMEDOUT);
rc = hclose(ch25);
errno_assert(rc == 0);
return 0;
}
