+++ /dev/null
-/*
- * robot_demo2011.c
- *
- * Robot's generic initialization and clean up functions.
- *
- * Copyright: (c) 2008 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define _XOPEN_SOURCE 500 /* For PTHREAD_PRIO_INHERIT etc. */
-
-#include <map.h>
-#include <movehelper.h>
-#include <pthread.h>
-#include <robomath.h>
-#include <robot.h>
-#include <robot_orte.h>
-#include <signal.h>
-#include <sys/time.h>
-#include <time.h>
-#include <unistd.h>
-#include "map_handling.h"
-#include <string.h>
-#include "actuators.h"
-#include <robodim.h>
-#include <ul_log.h>
-
-UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */
-
-
-#define MOTION_CONTROL_INIT_ONLY
-#include "motion-control.h"
-#include "robot.h"
-
-/* Global definition of robot structure */
-struct robot robot;
-
-#ifdef CONFIG_LOCK_CHECKING
-struct lock_log robot_lock_log;
-#endif
-
-static void block_signals()
-{
- sigset_t sigset;
- int i;
-
- sigemptyset(&sigset);
- for (i=SIGRTMIN; i<=SIGRTMAX; i++)
- sigaddset(&sigset, i);
-
- pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL);
-}
-
-static void int_handler(int sig)
-{
- robot_exit();
-}
-
-void fill_in_known_areas_in_map()
-{
- /* No known obstacles */
-}
-
-static void trans_callback(struct robo_fsm *fsm)
-{
- if (fsm == &robot.fsm.main) {
- strncpy(robot.orte.fsm_main.state_name, fsm->state_name, sizeof(robot.orte.fsm_main.state_name));
- ORTEPublicationSend(robot.orte.publication_fsm_main);
- } else if (fsm == &robot.fsm.motion) {
- strncpy(robot.orte.fsm_motion.state_name, fsm->state_name, sizeof(robot.orte.fsm_motion.state_name));
- ORTEPublicationSend(robot.orte.publication_fsm_motion);
- } else if (fsm == &robot.fsm.act) {
- strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name));
- ORTEPublicationSend(robot.orte.publication_fsm_act);
- }
-
-}
-
-/**
- * Initializes the robot.
- * Setup fields in robot structure, initializes FSMs and ORTE.
- *
- * @return 0
- */
-int robot_init()
-{
- int rv;
- pthread_mutexattr_t mattr;
- rv = pthread_mutexattr_init(&mattr);
-#ifdef HAVE_PRIO_INHERIT
- rv = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
-#endif
- pthread_mutex_init(&robot.lock, &mattr);
- pthread_mutex_init(&robot.lock_ref_pos, &mattr);
- pthread_mutex_init(&robot.lock_est_pos_odo, &mattr);
- pthread_mutex_init(&robot.lock_est_pos_indep_odo, &mattr);
- pthread_mutex_init(&robot.lock_meas_angles, &mattr);
- pthread_mutex_init(&robot.lock_joy_data, &mattr);
- pthread_mutex_init(&robot.lock_disp, &mattr);
-
- fsm_main_loop_init(&robot.main_loop);
-
- /* FSM initialization */
- fsm_init(&robot.fsm.main, "MAIN", &robot.main_loop);
- fsm_init(&robot.fsm.motion, "\tmot", &robot.main_loop);
- fsm_init(&robot.fsm.act, "\tACT", &robot.main_loop);
- robot.fsm.main.transition_callback = trans_callback;
- robot.fsm.act.transition_callback = trans_callback;
- robot.fsm.motion.transition_callback = trans_callback;
-
- robot.team_color = VIOLET;
-
- if (robot.team_color == VIOLET) {
- ul_loginf("We are VIOLET!\n");
- } else {
- ul_loginf("We are RED!\n");
- }
-
- robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));
-
- robot.ignore_hokuyo = false;
- robot.map = ShmapInit(1);
- fill_in_known_areas_in_map();
-
- signal(SIGINT, int_handler);
- signal(SIGTERM, int_handler);
- block_signals();
-
- /* initial values */
- robot.orte.motion_speed.left = 0;
- robot.orte.motion_speed.right = 0;
-
- robot.orte.pwr_ctrl.voltage33 = 1;
- robot.orte.pwr_ctrl.voltage50 = 1;
- robot.orte.pwr_ctrl.voltage80 = 1;
-
- robot.orte.camera_control.on = true;
-
- robot.fsm.motion.state = &fsm_state_motion_init;
-
- /* Only activate display if it is configured */
- /* FIXME: display
- robot.sercom = uoled_init(serial_comm);
- if (strcmp(robot.sercom->devname, "/dev/null") != 0)
- robot.fsm.display.state = &fsm_state_disp_init;
- */
-
- robot.obstacle_avoidance_enabled = true;
- robot.use_back_bumpers = true;
- robot.use_left_bumper = true;
- robot.use_right_bumper = true;
- robot.start_state = POWER_ON;
- robot.check_turn_safety = true;
-
- /* init ORTE domain, create publishers, subscribers, .. */
- rv = robot_init_orte();
- act_init(&robot.orte);
-
- return rv;
-}
-
-/**
- * Starts the robot FSMs and threads.
- *
- * @return 0
- */
-int robot_start()
-{
- int rv = 0;
-
- pthread_attr_t tattr;
- struct sched_param param;
- pthread_t thr_obstacle_forgeting;
- int ret;
-
- ret = motion_control_init();
- if(ret) {
- perror("motion_control_init");
- robot_exit();
- }
-
-
- /* Obstacle forgeting thread */
- pthread_attr_init (&tattr);
- pthread_attr_getschedparam (&tattr, ¶m);
- pthread_attr_getschedparam (&tattr, ¶m);
- pthread_attr_setschedpolicy(&tattr, SCHED_FIFO);
- param.sched_priority = OBST_FORGETING_PRIO;
- rv = pthread_attr_setschedparam (&tattr, ¶m);
- if (rv) {
- perror("robot_start: pthread_attr_setschedparam()");
- goto err;
- }
- rv = pthread_create(&thr_obstacle_forgeting,
- &tattr, thread_obstacle_forgeting, NULL);
- if (rv) {
- perror("robot_start: pthread_create");
- goto err;
- }
-
- sem_init(&robot.start, 0, 0);
-
- fsm_main_loop(&robot.main_loop);
-
-err:
- return rv;
-}
-
-/**
- * Signals all the robot threads to finish.
- */
-void robot_exit()
-{
- /* stop FSMs */
- fsm_exit(&robot.fsm.main);
- fsm_exit(&robot.fsm.motion);
- fsm_exit(&robot.fsm.act);
-}
-
-/**
- * Stops the robot. All resources alocated by robot_init() or
- * robot_start() are dealocated here.
- */
-void robot_destroy()
-{
- motion_control_done();
-
- // FIXME: set actuators to well defined position (FJ)
-
- robottype_roboorte_destroy(&robot.orte);
-
- fsm_destroy(&robot.fsm.main);
- fsm_destroy(&robot.fsm.motion);
- fsm_destroy(&robot.fsm.act);
- ShmapFree();
- ul_logdeb("robofsm: stop.\n");
-}
-
-void robot_get_est_pos_trans(double *x, double *y, double *phi)
-{
- robot_get_est_pos(x, y, phi);
- *x = __trans_x(*x);
- *y = __trans_y(*y);
- *phi = __trans_ang(*phi);
-}
-
-void robot_get_est_pos(double *x, double *y, double *phi)
-{
- if (robot.indep_odometry_works) {
- ROBOT_LOCK(est_pos_indep_odo);
- *x = robot.est_pos_indep_odo.x;
- *y = robot.est_pos_indep_odo.y;
- *phi = robot.est_pos_indep_odo.phi;
- ROBOT_UNLOCK(est_pos_indep_odo);
- } else if (robot.odometry_works) {
- ROBOT_LOCK(est_pos_odo);
- *x = robot.est_pos_odo.x;
- *y = robot.est_pos_odo.y;
- *phi = robot.est_pos_odo.phi;
- ROBOT_UNLOCK(est_pos_odo);
- } else {
- ROBOT_LOCK(ref_pos);
- *x = robot.ref_pos.x;
- *y = robot.ref_pos.y;
- *phi = robot.ref_pos.phi;
- ROBOT_UNLOCK(ref_pos);
- }
-}
+++ /dev/null
-/**
- * @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-2010 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#include <orte.h>
-#include <roboorte_robottype.h>
-#include "robodata.h"
-#include <robot.h>
-#include <movehelper.h>
-#include <math.h>
-#include <robomath.h>
-#include "map_handling.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 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_odo_cb(const ORTESendInfo *info, void *vinstance,
- void *sendCallBackParam)
-{
- struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
-
- 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 *sendCallBackParam)
-{
-}
-
-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_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct odo_data_type *instance = (struct odo_data_type *)vinstance;
- double dleft, dright, dtang, dphi;
- static bool first_run = true;
- /* spocitat prevodovy pomer */
- 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 (first_run) {
- ROBOT_LOCK(odo_data);
- robot.odo_data = *instance;
- ROBOT_UNLOCK(odo_data);
- first_run = false;
- break;
- }
-
- 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.indep_odometry_works = false;
- //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
- DBG("ORTE deadline occurred - odo_data receive\n");
- break;
- }
-}
-
-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;
- dleft = ((instance->right - robot.motion_irc.right) >> 8) * c;
-
- 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:
- robot.odometry_works = false;
- robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
- DBG("ORTE deadline occurred - motion_irc receive\n");
- break;
- }
-}
-
-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_speed receive\n");
- break;
- }
-}
-
-void rcv_motion_status_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");
- break;
- }
-}
-
-void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- switch (info->status) {
- case NEW_DATA:
- robot.status[COMPONENT_POWER]=STATUS_OK;
- break;
- case DEADLINE:
- robot.status[COMPONENT_POWER]=STATUS_FAILED;
- DBG("ORTE deadline occurred - pwr_voltage receive\n");
- break;
- }
-}
-
-void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- switch (info->status) {
- case NEW_DATA:
- break;
- case DEADLINE:
- DBG("ORTE deadline occurred - pwr_ctrl receive\n");
- break;
- }
-}
-
-void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
- static struct robot_cmd_type last_instance;
-
- switch (info->status) {
- case NEW_DATA:
- /* 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 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: {
- /* 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;
- break;
- case DEADLINE:
- robot.status[COMPONENT_START] = STATUS_FAILED;
- DBG("ORTE deadline occurred - robot_cmd receive\n");
- break;
- }
-}
-
-void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
-
- switch (info->status) {
- case NEW_DATA: {
- ROBOT_LOCK(hokuyo);
- robot.hokuyo = *instance;
- robot.status[COMPONENT_HOKUYO] = STATUS_OK;
- ROBOT_UNLOCK(hokuyo);
- 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;
- }
-}
-
-// 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: {
- 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:
- 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_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- 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:
- // 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:
- robot.status[COMPONENT_JAWS] = STATUS_FAILED;
- DBG("ORTE deadline occurred - actuator_status receive\n");
- break;
- }
-}
-
-void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- 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_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
- static bool last_strategy;
- switch (info->status) {
- case NEW_DATA:
- robot.team_color = instance->team_color;
-
- if (!last_strategy && instance->strategy) {
- /* strategy switching */
- FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
- }
- last_strategy = instance->strategy;
- break;
- case DEADLINE:
- break;
- }
-}
-
-void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
- static bool last_left, last_right;
- switch (info->status) {
- 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:
- break;
- }
-}
-
-#define HIST_CNT 5
-#if 0
-static int cmp_double(const void *v1, const void *v2)
-{
- const double *d1 = v1, *const d2 = v2;
- if (d1 < d2)
- return -1;
- else if (d1 > d2)
- return +1;
- else
- return 0;
-}
-#endif
-
-int robot_init_orte()
-{
- int rv = 0;
-
- robot.orte.strength = 20;
-
- rv = robottype_roboorte_init(&robot.orte);
- if (rv)
- return rv;
-
- /* creating publishers */
- robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
- robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
- 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_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_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
- robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_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;
-}
-
projects / eurobot / public.git / commitdiff