+/**
+ * @file robot_orte.c
+ * @date 2008-2010
+ *
+ * @brief Module providing communication through ORTE
+ */
+
/*
* robot_orte.c 08/04/21
*
* Robot's orte stuffs.
*
- * Copyright: (c) 2008 CTU Dragons
+ * Copyright: (c) 2008-2010 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
*/
#include <orte.h>
#include <roboorte_robottype.h>
-#include <roboorte.h>
-#include <robodata.h>
+#include "robodata.h"
#include <robot.h>
#include <movehelper.h>
#include <math.h>
#include <robomath.h>
-#include <laser-nav.h>
#include "map_handling.h"
-#include <oledlib.h>
+#include "match-timing.h"
#include <string.h>
+#include <can_msg_def.h>
+#include <actuators.h>
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
+
+#ifdef ORTE_DEBUG
+ #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
+#else
+ #define DBG(format, ...)
+#endif
+
+/*****FIXME:*****/
+extern sem_t measurement_received;
/* ----------------------------------------------------------------------
* PUBLISHER CALLBACKS - GENERIC
void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
- struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
+ struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
ROBOT_LOCK(ref_pos);
*instance = robot.ref_pos;
ROBOT_UNLOCK(ref_pos);
}
-void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
+void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
void *sendCallBackParam)
{
- struct est_pos_type *instance = (struct est_pos_type *)vinstance;
+ struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
- ROBOT_LOCK(est_pos);
- *instance = robot.est_pos;
- ROBOT_UNLOCK(est_pos);
+ ROBOT_LOCK(est_pos_odo);
+ *instance = robot.est_pos_odo;
+ ROBOT_UNLOCK(est_pos_odo);
+}
+
+void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+ struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
+
+ ROBOT_LOCK(est_pos_indep_odo);
+ *instance = robot.est_pos_indep_odo;
+ ROBOT_UNLOCK(est_pos_indep_odo);
+}
+
+static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+ struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
+
+ robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
}
void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
{
}
+void send_match_time_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+ struct match_time_type *instance = (struct match_time_type *)vinstance;
+
+ if (robot.start_state == POWER_ON || robot.start_state == START_PLUGGED_IN) {
+ instance->time = 90;
+ } else {
+ instance->time = 90 - robot_current_time();
+ }
+}
/* ----------------------------------------------------------------------
* SUBSCRIBER CALLBACKS - GENERIC
* ---------------------------------------------------------------------- */
-
-void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
- static struct motion_irc_type prevInstance;
- static int firstRun = 1;
+ struct odo_data_type *instance = (struct odo_data_type *)vinstance;
+ double dleft, dright, dtang, dphi;
+ static bool first_run = true;
/* spocitat prevodovy pomer */
- double n = (double)(28.0 / 1.0);
- /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
- double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
-
- double aktk0 = 0;
- double aktk1 = 0;
- double deltaU = 0;
- double deltAlfa = 0;
+ const double n = (double)(1.0 / 1.0);
+
+ /* vzdalenost na pulz IRC */
+ const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
switch (info->status) {
case NEW_DATA:
- if(firstRun) {
- prevInstance = *instance;
- firstRun = 0;
+ if (first_run) {
+ ROBOT_LOCK(odo_data);
+ robot.odo_data = *instance;
+ ROBOT_UNLOCK(odo_data);
+ first_run = false;
break;
}
- aktk0 = ((prevInstance.left - instance->left) >> 8) * c;
- aktk1 = ((instance->right - prevInstance.right) >> 8) * c;
- prevInstance = *instance;
-
- deltaU = (aktk0 + aktk1) / 2;
- deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
-
- struct mcl_robot_odo *odo = malloc(sizeof(struct mcl_robot_odo));
- memset(odo, 0, sizeof(*odo));
- odo->dx = deltaU;
- odo->dy = 0;
- odo->dangle = deltAlfa;
- FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo);
- robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
+ dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a; // TODO >> 8 ?
+ dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
+
+ dtang = (dleft + dright) / 2.0;
+ dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
+
+ ROBOT_LOCK(est_pos_indep_odo);
+ robot.odo_distance_a +=dleft;
+ robot.odo_distance_b +=dright;
+ double a = robot.est_pos_indep_odo.phi;
+ robot.est_pos_indep_odo.x += dtang*cos(a);
+ robot.est_pos_indep_odo.y += dtang*sin(a);
+ robot.est_pos_indep_odo.phi += dphi;
+ ROBOT_UNLOCK(est_pos_indep_odo);
+
+ ROBOT_LOCK(odo_data);
+ robot.odo_data = *instance;
+ ROBOT_UNLOCK(odo_data);
+
+ robot.indep_odometry_works = true;
+
+ /* wake up motion-control/thread_trajectory_follower */
+ sem_post(&measurement_received);
+
+ //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
break;
case DEADLINE:
- robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
- DBG("ORTE deadline occurred - motion_irc receive\n");
+ robot.indep_odometry_works = false;
+ //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - odo_data receive\n");
break;
}
}
-void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
+ struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
+ double dleft, dright, dtang, dphi;
+ static bool first_run = true;
+ /* spocitat prevodovy pomer */
+ const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0);
+
+ /* vzdalenost na pulz IRC */
+ const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION);
switch (info->status) {
case NEW_DATA:
+ if (first_run) {
+ ROBOT_LOCK(motion_irc);
+ robot.motion_irc = *instance;
+ ROBOT_UNLOCK(motion_irc);
+ first_run = false;
+ break;
+ }
+
+
+ /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright),
+ what is the right solution?
+ This was neccessary to view motor odometry correctly in robomon. */
+ dright = ((robot.motion_irc.left - instance->left) >> 8) * c * robot.odo_cal_b;
+ dleft = ((instance->right - robot.motion_irc.right) >> 8) * c * robot.odo_cal_a;
+
+ dtang = (dleft + dright) / 2.0;
+ dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
+
+ ROBOT_LOCK(est_pos_odo);
+ double a = robot.est_pos_odo.phi;
+ robot.est_pos_odo.x += dtang*cos(a);
+ robot.est_pos_odo.y += dtang*sin(a);
+ robot.est_pos_odo.phi += dphi;
+ ROBOT_UNLOCK(est_pos_odo);
+
+ /* locking should not be needed, but... */
+ ROBOT_LOCK(motion_irc);
+ robot.motion_irc = *instance;
+ robot.motion_irc_received = 1;
+ ROBOT_UNLOCK(motion_irc);
+
+ robot.odometry_works = true;
+
+ robot.status[COMPONENT_MOTOR] = STATUS_OK;
break;
case DEADLINE:
- DBG("ORTE deadline occurred - motion_speed receive\n");
+ robot.odometry_works = false;
+ robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - motion_irc receive\n");
break;
}
}
-void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
switch (info->status) {
case NEW_DATA:
break;
case DEADLINE:
- DBG("ORTE deadline occurred - motion_status receive\n");
+ DBG("ORTE deadline occurred - motion_speed receive\n");
break;
}
}
-void rcv_joy_data_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct joy_data_type *instance = (struct joy_data_type *)vinstance;
-
switch (info->status) {
case NEW_DATA:
- ROBOT_LOCK(joy_data);
- robot.joy_data = *instance;
- ROBOT_UNLOCK(joy_data);
break;
case DEADLINE:
- DBG("ORTE deadline occurred - joy_data receive\n");
+ DBG("ORTE deadline occurred - motion_status receive\n");
break;
}
}
{
switch (info->status) {
case NEW_DATA:
- robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
+ robot.status[COMPONENT_POWER]=STATUS_OK;
break;
case DEADLINE:
- robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
+ robot.status[COMPONENT_POWER]=STATUS_FAILED;
DBG("ORTE deadline occurred - pwr_voltage receive\n");
break;
}
{
struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
static struct robot_cmd_type last_instance;
- enum robot_state state;
switch (info->status) {
case NEW_DATA:
- if (instance->start == last_instance.start)
- break;
-
- ROBOT_LOCK(state);
- state = robot.state;
- ROBOT_UNLOCK(state);
- printf("in state=%d \n", state);
-
- switch (state) {
- case JUST_STARTED:
- if (instance->start)
- break;
- FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
- state = LASER_STARTED;
+ /* Stupid way of controlling the robot by
+ * pluggin in and out start connector. */
+ switch (robot.start_state) {
+ case POWER_ON:
+ if (!instance->start_condition) {
+ robot.status[COMPONENT_START] = STATUS_WARNING;
+ robot.start_state = START_PLUGGED_IN;
+ ul_logmsg("START_PLUGGED_IN\n");
+ }
break;
- case LASER_STARTED:
- if (!instance->start)
- break;
- FSM_SIGNAL(MAIN, EV_START, NULL);
- state = COMPETITION_STARTED;
+ case START_PLUGGED_IN:
+ robot.status[COMPONENT_START] = STATUS_OK;
+ /* Competition starts when plugged out */
+ if (instance->start_condition) {
+ FSM_SIGNAL(MAIN, EV_START, NULL);
+ robot.start_state = COMPETITION_STARTED;
+ ul_logmsg("STARTED\n");
+ }
break;
- case COMPETITION_STARTED:
- if (!instance->start) {
- robot_exit();
+ case COMPETITION_STARTED: {
+ /* Subsequent plug-in stops the robot */
+ static int num = 0;
+ if (!instance->start_condition) {
+ robot.status[COMPONENT_START] = STATUS_WARNING;
+ if (++num == 10)
+ robot_exit();
}
break;
+ }
}
last_instance = *instance;
- printf("out state=%d \n", state);
- ROBOT_LOCK(state);
- robot.state = state;
- ROBOT_UNLOCK(state);
break;
case DEADLINE:
- DBG("ORTE deadline occurred - pwr_ctrl receive\n");
+ robot.status[COMPONENT_START] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - robot_cmd receive\n");
break;
}
}
case NEW_DATA: {
ROBOT_LOCK(hokuyo);
robot.hokuyo = *instance;
+ robot.status[COMPONENT_HOKUYO] = STATUS_OK;
ROBOT_UNLOCK(hokuyo);
- update_map_hokuyo(instance);
+ if(!robot.ignore_hokuyo)
+ {
+ update_map_hokuyo(instance);
+ }
break;
}
case DEADLINE:
+ robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
+ //system("killall -9 hokuyo");
DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
break;
}
}
-/* ----------------------------------------------------------------------
- * SUBSCRIBER CALLBACKS - EB2008
- * ---------------------------------------------------------------------- */
-void rcv_servos_cb(const ORTERecvInfo *info, void *vinstance,
+// FIXME: this is not up to date (Filip, 2010)
+void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
+ struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+
switch (info->status) {
- case NEW_DATA:
+ case NEW_DATA: {
+ ROBOT_LOCK(camera_result);
+ robot.corns_conf_side = instance->side;
+ robot.corns_conf_center = instance->center;
+ ROBOT_UNLOCK(camera_result);
+ robot.status[COMPONENT_CAMERA] = STATUS_OK;
break;
+ }
case DEADLINE:
- DBG("ORTE deadline occurred - servos receive\n");
+ if (robot.orte.camera_control.on) {
+ robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
+ //system("killall -9 rozpuk");
+ }
+
+ DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
break;
}
}
+/* ----------------------------------------------------------------------
+ * SUBSCRIBER CALLBACKS - EB2008
+ * ---------------------------------------------------------------------- */
-void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
- struct drives_type *instance = (struct drives_type *)vinstance;
-
+ struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
+ static int last_response_left = 0;
+ static int last_response_right = 0;
switch (info->status) {
case NEW_DATA:
- ROBOT_LOCK(drives);
- robot.drives = *instance;
- ROBOT_UNLOCK(drives);
+ // new data arrived and requested position equals actual position
+ if (instance->flags.left == 0 &&
+ instance->flags.right == 0)
+ robot.status[COMPONENT_JAWS] = STATUS_OK;
+ else
+ robot.status[COMPONENT_JAWS] = STATUS_WARNING;
+
+ if (instance->response.left != last_response_left &&
+ instance->response.right != last_response_right &&
+ instance->response.left == act_jaw_left_get_last_reqest() &&
+ instance->response.left == act_jaw_right_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
+
+ last_response_left = instance->response.left;
+ last_response_right = instance->response.right;
break;
case DEADLINE:
- DBG("ORTE deadline occurred - drives receive\n");
+ robot.status[COMPONENT_JAWS] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - actuator_status receive\n");
break;
}
}
-void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
- struct laser_data_type *instance = (struct laser_data_type *)vinstance;
-
- switch (info->status) {
- case NEW_DATA: {
- struct mcl_laser_measurement *meas_angles;
- meas_angles = malloc(sizeof(*meas_angles));
- memset(meas_angles, 0, sizeof(*meas_angles));
-
- meas_angles->count = 1;
- meas_angles->val[0] =
- (double)TIME2ANGLE(instance->period,
- instance->measure);
-// DBG("a=%f\n", RAD2DEG(robot.meas_angles.val[0]));
-
- bool sent;
- FSM_SIGNAL_TIMED(LOC, EV_LASER_RECEIVED, meas_angles, 20/*ms*/, &sent);
- if (!sent) {
- DBG("%s: MCL is busy - not updating\n", __FUNCTION__);
- free(meas_angles);
- }
- robot.hw_status[STATUS_LASER] = HW_STATUS_OK;
- break;
- }
- case DEADLINE:
- robot.hw_status[STATUS_LASER] = HW_STATUS_FAILED;
- DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
- break;
- }
+ struct lift_status_type *instance = (struct lift_status_type *)vinstance;
+ static int last_response = 0;
+ switch (info->status) {
+ case NEW_DATA:
+ // new data arrived and requested position equals actual position
+ if (instance->flags == 0)
+ robot.status[COMPONENT_LIFT] = STATUS_OK;
+ else
+ robot.status[COMPONENT_LIFT] = STATUS_WARNING;
+
+ if (instance->response != last_response &&
+ instance->response == act_lift_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
+ last_response = instance->response;
+ break;
+ case DEADLINE:
+ robot.status[COMPONENT_LIFT] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - actuator_status receive\n");
+ break;
+ }
}
-void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
- struct cmu_type *instance = (struct cmu_type *)vinstance;
- static enum ball_color last_color = NO_BALL;
- static unsigned char first = 1;
-
+ struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
+ static bool last_strategy;
switch (info->status) {
- case NEW_DATA: {
- ROBOT_LOCK(cmu);
- robot.cmu = *instance;
- ROBOT_UNLOCK(cmu);
- if (first) {
- last_color = robot.cmu.color;
- first = 0;
- }
- if (robot.cmu.color != NO_BALL) {
- if (last_color != robot.cmu.color) {
- last_color = robot.cmu.color;
- FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
- }
+ case NEW_DATA:
+ robot.team_color = instance->team_color;
+
+ if (!last_strategy && instance->strategy) {
+ /* strategy switching */
+ FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
}
- robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
+ last_strategy = instance->strategy;
break;
- }
case DEADLINE:
- robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
- DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
break;
}
}
-void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
- struct bumper_type *instance = (struct bumper_type *)vinstance;
-
+ struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
+ static bool last_left, last_right;
switch (info->status) {
- case NEW_DATA: {
- ROBOT_LOCK(bumper);
- robot.bumper = *instance;
- ROBOT_UNLOCK(bumper);
+ case NEW_DATA:
+ if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
+ FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+
+ if (instance->bumper_left || instance->bumper_right) {
+ FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
+ }
break;
- }
case DEADLINE:
- DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
break;
}
}
#define HIST_CNT 5
-
+#if 0
static int cmp_double(const void *v1, const void *v2)
{
const double *d1 = v1, *const d2 = v2;
else
return 0;
}
-
-static inline double *sharp_ptr(struct sharps_type *sharps, int index)
-{
- switch (index) {
- case 0: return &sharps->front_left; break;
- case 1: return &sharps->front_right; break;
- case 2: return &sharps->left; break;
- case 3: return &sharps->right; break;
- default: return &sharps->front_left; break;
- }
-
-}
-
-void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct sharps_type *instance = (struct sharps_type *)vinstance;
- static struct sharps_type history[HIST_CNT];
- static double history_sorted[HIST_CNT];
- static int ind = -1;
- int i, s;
- //struct sharps_type *for_mcl;
-
- switch (info->status) {
- case NEW_DATA: {
- ROBOT_LOCK(sharps);
- robot.sharps = *instance;
- ROBOT_UNLOCK(sharps);
- if (ind == -1) {
- for (i=0; i<HIST_CNT; i++) {
- history[i].front_left = 0.8;
- history[i].front_right = 0.8;
- history[i].left = 0.8;
- history[i].right = 0.8;
- }
- }
- ind++;
- if (ind >= HIST_CNT) {
- ind = 0;
- }
- history[ind] = *instance;
-
- for (s=0; s<4; s++) {
- for (i=0; i<HIST_CNT; i++) {
- history_sorted[i] = *sharp_ptr(&history[i], s);
- }
- qsort(history_sorted, HIST_CNT, sizeof(history_sorted[0]), cmp_double);
- *sharp_ptr(instance, s) = history_sorted[HIST_CNT/2]; /* Get median */
- }
-
- update_map(instance);
- break;
- }
- case DEADLINE:
- DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
- break;
- }
-}
+#endif
int robot_init_orte()
{
return rv;
/* creating publishers */
- robottype_publisher_motion_irc_create(&robot.orte, NULL, &robot.orte);
robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
- robottype_publisher_motion_status_create(&robot.orte, NULL, &robot.orte);
robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
- robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
- robottype_publisher_joy_data_create(&robot.orte, NULL, &robot.orte);
- robottype_publisher_pwr_voltage_create(&robot.orte, NULL, &robot.orte);
- robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
-
- robottype_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
- robottype_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
- robottype_publisher_laser_cmd_create(&robot.orte, NULL, NULL);
+ robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
+ robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
+ robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
+ robottype_publisher_match_time_create(&robot.orte, send_match_time_cb, &robot.orte);
+ //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
+
+ // I didn't know what was the difference between the callback function pointer
+ // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
+ // IS CRUCIAL!
+ // - NULL: message is published only when OrtePublicationSend called
+ // - pointer to empty function: message is published periodically
+ robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
/* create generic subscribers */
+ robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
- robottype_subscriber_joy_data_create(&robot.orte, rcv_joy_data_cb, &robot.orte);
robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
- robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
+ //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
-
- /* create eb2008 subscribers */
- robottype_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
- robottype_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
- robottype_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
- robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
- robottype_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
- robottype_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);
+ robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
+ robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
+ robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
+ robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
+ robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
return rv;
}