ersp.cc

Go to the documentation of this file.

/*
 * ERSP  Player driver
 */

#include "ersp.h"
#include "log.h"


// Driver setup and shutdown

ERSP::ERSP(ConfigFile* cf, int section)
      : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
{
   // zero ids, so that we'll know later which interfaces were requested
   memset(&this->position_id, 0, sizeof(player_devaddr_t));
   memset(&this->bumper_id, 0, sizeof(player_devaddr_t));
   memset(&this->ir_range_id, 0, sizeof(player_devaddr_t));
   memset(&this->ir_binary_id, 0, sizeof(player_devaddr_t));
   memset(&this->power_id, 0, sizeof(player_devaddr_t));

   this->position_subscriptions = 0;
   this->bumper_subscriptions = 0;
   this->ir_range_subscriptions = 0;
   this->ir_binary_subscriptions = 0;
   this->power_subscriptions = 0;
   this->running = false;

   if (cf->ReadDeviceAddr(&(this->position_id), section, "provides",
                     PLAYER_POSITION2D_CODE, -1, NULL) == 0) {
      if (this->AddInterface(this->position_id) != 0) {
         this->SetError(-1);
         return;
      }
   }
 
   if (cf->ReadDeviceAddr(&(this->bumper_id), section, "provides",
                     PLAYER_BUMPER_CODE, -1, "bumper") == 0) {
      if (this->AddInterface(this->bumper_id) != 0) {
         this->SetError(-1);
         return;
      }
   }

   if (cf->ReadDeviceAddr(&(this->ir_range_id), section, "provides",
                     PLAYER_IR_CODE, -1, NULL) == 0) {
      if (this->AddInterface(this->ir_range_id) != 0) {
         this->SetError(-1);
         return;
      }
   }

   if (cf->ReadDeviceAddr(&(this->ir_binary_id), section, "provides",
                     PLAYER_BUMPER_CODE, -1, "ir") == 0) {
      if (this->AddInterface(this->ir_binary_id) != 0) {
         this->SetError(-1);
         return;
      }
   }

#ifdef POWER
   if (cf->ReadDeviceAddr(&(this->power_id), section, "provides",
                     PLAYER_POWER_CODE, -1, NULL) == 0) {
      if (this->AddInterface(this->power_id) != 0) {
         this->SetError(-1);
         return;
      }
   }
#endif
}

int ERSP::SetupERSP()
{
   static const struct ersp_dev ersp_devs[] = {

#define ERSP_INTERFACE(ROBOT, TYPE, NAME)
#define ERSP_INTERFACE_END
#define ERSP_DEV(ROBOT, ID, TYPE, NAME) \
       { ROBOT##_##TYPE##_##ID, ROBOT##_##ID, ERSP_##TYPE, NAME },

#include ERSP_INCLUDE

#undef ERSP_INTERFACE
#undef ERSP_INTERFACE_END
#undef ERSP_DEV

   };
   devs = ersp_devs;

   if (this->running)
      return 0;

#ifdef HAVE_ERSP
   Evolution::Result result;

   // Get the ERSP resource manager
   manager = new Evolution::ResourceManager(NULL, &result); 
   if (result != Evolution::RESULT_SUCCESS) {
      LOG_ERR("Cannot create a resource manager.");
      return -1;
   }

   result = manager->get_resource_container(0, &container);
   if (result != Evolution::RESULT_SUCCESS) {
      LOG_ERR("Cannot create a resource container.");
      return -1;
   }

   for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
      /* XXX: Ordered by ersp_dev_type_t. */
      static const char *interfaces[] = {
         Evolution::IDriveSystem::INTERFACE_ID,
         Evolution::IAvoidance::INTERFACE_ID,
         Evolution::IBattery::INTERFACE_ID,
         Evolution::IBumpSensor::INTERFACE_ID,
         Evolution::IBumpSensor::INTERFACE_ID,
         Evolution::IRangeSensor::INTERFACE_ID,
      };
      void **handle = &devices[i].handle;
      const char *id = devs[i].id;
      const char *interface = interfaces[devs[i].type];

      result = container->obtain_interface(Evolution::NO_TICKET,
                                  id, interface, handle);
      if (result != Evolution::RESULT_SUCCESS) {
         LOG_ERR("Cannot create a %s system.", id);
         return -1;
      }
   }

   devices[SCORPION_AVOID_AVOID].avoid->disable_avoidance(Evolution::NO_TICKET);
#endif /* HAVE_ERSP */

   return 0;
}

/* Should return 1 on error, and 0 on success. */
int ERSP::Setup()
{
   LOG_INFO("Driver setup");

   if (SetupERSP()) {
#ifdef HAVE_ERSP
      // FIXME: This might need to be changed so that
      // nothing is freed until the destructor is called
      // and instead have the SetupERSP conditionally
      // initialize variables.
      if (manager != NULL)
         delete manager;
#endif /* HAVE_ERSP */

      return 1;
   }

   // Now spawn reading thread
   // Prospone this to here since ::Shutdown will not be called if
   // an error condition is returned.
   this->StartThread();
   this->running = true;

   LOG_INFO("Driver setup done");

    return 0;
}

int ERSP::Shutdown()
{
   LOG_INFO("Driver shutdown");

   if (!this->running)
      return 0;

   this->StopThread();
   this->running = false;

   LOG_INFO("Driver has been shutdown");

   return 0;
}

ERSP::~ERSP (void)
{
#ifdef HAVE_ERSP
   /* Deallocate the Resource Manager.
    *
    * This means that the ERSP library will be initialized and ready
    * throughout the life of the player server.
    *
    * FIXME: We still need to ensure that the various interfaces
    * (driver, ...) are reset so the robot doesn't do anything. */

   if (manager != NULL)
      delete manager;
#endif /* HAVE_ERSP */
}

void
ERSP::Main()
{
#ifdef HAVE_ERSP
   Evolution::Timestamp timestamp;
   Evolution::Result result;
#endif /* HAVE_ERSP */
   time_t prev_time = 0;

   for(;;) {
      pthread_testcancel();

      this->Lock();

      if (this->bumper_subscriptions != 0) {
         player_bumper_data_t *bumper = &ersp_data.bumper;

         memset(bumper, 0, sizeof(*bumper));

         for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
            bool triggered = false;

            if (devs[i].type != ERSP_BUMP)
               continue;

#ifdef HAVE_ERSP
            result = devices[i].bumper->is_triggered(Evolution::NO_TICKET,
                                                         &timestamp,
                                           &triggered);
            if (result != Evolution::RESULT_SUCCESS)
               LOG_ERR("Error reading from binary sensor %s",
                     devs[i].id);
#endif /* HAVE_ERSP */

            bumper->bumpers[bumper->bumpers_count++] = (uint8_t) triggered == true;
         }
      }

      if (this->ir_range_subscriptions != 0) {
         player_ir_data_t *ir = &ersp_data.ir_range;

         memset(ir, 0, sizeof(*ir));

         for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
            double distance = 0;

            if (devs[i].type != ERSP_IR_RANGE)
               continue;

#ifdef HAVE_ERSP
            result = devices[i].range->get_distance_reading(Evolution::NO_TICKET,
                                        &timestamp, &distance);
            if (result != Evolution::RESULT_SUCCESS)
               LOG_ERR("Error reading from range sensor %s",
                     devs[i].id);
#endif /* HAVE_ERSP */

            /* Convert to meters */
            distance = distance / 100;
            if (distance > IR_MAX_RANGE) {
               distance = IR_MAX_RANGE;
            }

            ir->ranges[ir->ranges_count++] = (float) distance;
         }
      }

      if (this->ir_binary_subscriptions != 0) {
         player_bumper_data_t *ir = &ersp_data.ir_binary;

         memset(ir, 0, sizeof(*ir));

         for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
            bool triggered = false;

            if (devs[i].type != ERSP_IR_BINARY)
               continue;

#ifdef HAVE_ERSP
            result = devices[i].bumper->is_triggered(Evolution::NO_TICKET,
                                                         &timestamp,
                                           &triggered);
            if (result != Evolution::RESULT_SUCCESS)
               LOG_ERR("Error reading from binary sensor %s",
                     devs[i].id);
#endif /* HAVE_ERSP */

            ir->bumpers[ir->bumpers_count++] = (uint8_t) triggered == true;
         }
      }

#ifdef POWER
      if (this->power_subscriptions != 0) {
         player_power_data_t *power = &ersp_data.power;
         double percent_charge = 0, voltage = 0;

         memset(power, 0, sizeof(*power));
         power->valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;

#ifdef HAVE_ERSP
         result = devices[SCORPION_BATTERY_BATTERY].battery->get_percent_charge_left(Evolution::NO_TICKET,
            &timestamp, &percent_charge);
         if (result != Evolution::RESULT_SUCCESS)
            LOG_ERR("Error reading from battery");
#endif /* HAVE_ERSP */
         power->percent = (float) percent_charge;

#ifdef HAVE_ERSP
         result = devices[SCORPION_BATTERY_BATTERY].battery->get_voltage(Evolution::NO_TICKET,
            &timestamp, &voltage);
         if (result != Evolution::RESULT_SUCCESS)
            LOG_ERR("Error reading from battery");
#endif /* HAVE_ERSP */
         power->volts = (float) percent_charge;
      }
#endif

      this->Unlock();

      if (this->position_subscriptions ||
          this->bumper_subscriptions ||
         this->ir_range_subscriptions ||
         this->ir_binary_subscriptions ||
         this->power_subscriptions) {
         time_t now = time(NULL);

         /* Limit to only publishing every second. */
         if (now > prev_time) {
            prev_time = now;
         }
      }
            PutData();

      // Handle pending messages
      if (!this->InQueue->Empty()) {
         LOG_DBG("Processing messages");
         ProcessMessages();
      }
   }
}


// Message processing

int
ERSP::ProcessMessage(MessageQueue * resp_queue,
                player_msghdr * hdr,
                void * data)
{
   // Look for configuration requests
   if (hdr->type == PLAYER_MSGTYPE_REQ)
      return this->HandleConfig(resp_queue, hdr, data);

   if (hdr->type == PLAYER_MSGTYPE_CMD)
      return this->HandleCommand(hdr, data);

   return -1;
}

int
ERSP::HandleConfig(MessageQueue* resp_queue,
               player_msghdr * hdr,
               void * data)
{
   // check for position config requests
   if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                       PLAYER_POSITION2D_REQ_SET_ODOM,
                       this->position_id)) {

      player_position2d_set_odom_req_t* set_odom_req;

      if (hdr->size != sizeof(player_position2d_set_odom_req_t)) {
         LOG_WARN("Arg to odometry set requests wrong size; ignoring");
         return -1;
      }

      set_odom_req = (player_position2d_set_odom_req_t*) data;

      LOG_INFO("req set odom");

      this->Publish(this->position_id, resp_queue,
                 PLAYER_MSGTYPE_RESP_ACK,
                 PLAYER_POSITION2D_REQ_SET_ODOM);
      return 0;
   }
   else if (Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                           PLAYER_POSITION2D_REQ_MOTOR_POWER,
                           this->position_id)) {

      player_position2d_power_config_t* power_config;

      /* motor state change request
       *   1 = enable motors
       *   0 = disable motors (default)
       */
      if (hdr->size != sizeof(player_position2d_power_config_t)) {
         LOG_WARN("Arg to motor state change request wrong size; ignoring");
         return -1;
      }

      power_config = (player_position2d_power_config_t*) data;

      LOG_INFO("req motor power");

      this->Publish(this->position_id, resp_queue,
                 PLAYER_MSGTYPE_RESP_ACK,
                 PLAYER_POSITION2D_REQ_MOTOR_POWER);
      return 0;
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                           PLAYER_POSITION2D_REQ_RESET_ODOM,
                           this->position_id)) {
      /* reset position to 0,0,0: no args */
      if (hdr->size != 0) {
         PLAYER_WARN("Arg to reset position request is wrong size; ignoring");
         return(-1);
      }

      LOG_INFO("req reset odom");

      this->Publish(this->position_id, resp_queue,
                 PLAYER_MSGTYPE_RESP_ACK,
                 PLAYER_POSITION2D_REQ_RESET_ODOM);
      return 0;
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                           PLAYER_POSITION2D_REQ_GET_GEOM,
                           this->position_id)) {
      /* Return the robot geometry. */
      if (hdr->size != 0) {
         LOG_WARN("Arg get robot geom is wrong size; ignoring");
         return(-1);
      }

      player_position2d_geom_t geom;

      // FIXME
      memset(&geom, 0, sizeof(geom));

      LOG_INFO("req get geom");

      this->Publish(this->position_id, resp_queue,
                 PLAYER_MSGTYPE_RESP_ACK,
                 PLAYER_POSITION2D_REQ_GET_GEOM,
                 (void*) &geom, sizeof(geom), NULL);
      return 0;
   }
   else if (Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                           PLAYER_POSITION2D_REQ_VELOCITY_MODE,
                           this->position_id)) {

      player_position2d_velocity_mode_config_t* velmode_config;

      /* velocity control mode:
       *   0 = direct wheel velocity control (default)
       *   1 = separate translational and rotational control
       */
      if (hdr->size != sizeof(player_position2d_velocity_mode_config_t)) {
         LOG_WARN("Arg to velocity control mode change "
                "request is wrong size; ignoring");
         return -1;
      }

      velmode_config = (player_position2d_velocity_mode_config_t*) data;

      LOG_INFO("req velocity mode config");

      this->Publish(this->position_id, resp_queue,
                 PLAYER_MSGTYPE_RESP_ACK,
                 PLAYER_POSITION2D_REQ_VELOCITY_MODE);
      return 0;
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                           PLAYER_BUMPER_GET_GEOM,
                           this->bumper_id)) {
      /* Return the bumper geometry. */
      if(hdr->size != 0) {
         PLAYER_WARN("Arg get bumper geom is wrong size; ignoring");
         return -1;
      }

      player_bumper_geom_t geom;

      geom.bumper_def_count = 2;

      geom.bumper_def[0].pose.px = 0;
      geom.bumper_def[0].pose.py = 0;
      geom.bumper_def[0].pose.pa = 0;
      geom.bumper_def[0].length = 0.020;
      geom.bumper_def[0].radius = 0;
   
      geom.bumper_def[0].pose.px = 0;
      geom.bumper_def[0].pose.py = 0;
      geom.bumper_def[0].pose.pa = 0;
      geom.bumper_def[0].length = 0.020;
      geom.bumper_def[0].radius = 0;

      this->Publish(this->bumper_id, resp_queue,
                 PLAYER_MSGTYPE_RESP_ACK, PLAYER_BUMPER_GET_GEOM,
                 (void *) &geom, sizeof(geom), NULL);

      return 0;
   }
   else {
      LOG_WARN("unknown config request to ersp driver");
      return -1;
   }
}

int
ERSP::HandleCommand(player_msghdr *hdr, void* data)
{
   if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                       PLAYER_POSITION2D_CMD_VEL,
                       this->position_id)) {

      const double SPEED_ACCELERATION = 20;
      const double ANGULAR_ACCELERATION = M_PI / 2.0; // radians/sec^2
      player_position2d_cmd_vel_t position_cmd;
      double speed, turn_rate;

      // get and send the latest motor command
      position_cmd = *(player_position2d_cmd_vel_t*)data;

      // m/sec -> cm/sec
      speed = position_cmd.vel.px * 100;

      // rad/sec
      turn_rate = position_cmd.vel.pa;

      LOG_INFO("Handle velocity cmd: speed=%f, turn_rate=%f, sacc=%f, tacc=%f",
          speed, turn_rate, SPEED_ACCELERATION, ANGULAR_ACCELERATION);

#ifdef HAVE_ERSP
      // Player uses m/sec while ERSP uses cm/s
      devices[SCORPION_DRIVE_DRIVE].driver->move_and_turn(Evolution::NO_TICKET,
                           speed, SPEED_ACCELERATION,
                           turn_rate, ANGULAR_ACCELERATION);
#endif /* HAVE_ERSP */

      return 0;
   }
#if 0
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                           PLAYER_POSITION2D_CMD_VEL,
                           this->position_id)) {

      ersp_driver->move_and_turn(Evolution::NO_TICKET, speed,
                           0, turn_rate, 0);

      return 0;
   }
#endif

   return -1;
}


// Publish / Subscribe / Unsubscribe

int
ERSP::Subscribe(player_devaddr_t id)
{
   int result;

   // Do the subscription ...
   if ((result = Driver::Subscribe(id)) == 0) {
      // ... and increment the appropriate subscription counter
      if (Device::MatchDeviceAddress(id, this->position_id)) {
         this->position_subscriptions++;
         LOG_INFO("%d position subscriptions",
                this->position_subscriptions);
      }
      else if (Device::MatchDeviceAddress(id, this->bumper_id)) {
         this->bumper_subscriptions++;
         LOG_INFO("%d bumper subscriptions",
                this->bumper_subscriptions);
      }
      else if (Device::MatchDeviceAddress(id, this->ir_range_id)) {
         this->ir_range_subscriptions++;
         LOG_INFO("%d ir subscriptions",
                this->ir_range_subscriptions);
      }
      else if (Device::MatchDeviceAddress(id, this->ir_binary_id)) {
         this->ir_binary_subscriptions++;
         LOG_INFO("%d ir subscriptions",
                this->ir_binary_subscriptions);
      }
#ifdef POWER
      else if (Device::MatchDeviceAddress(id, this->power_id)) {
         this->power_subscriptions++;
         LOG_INFO("%d power subscriptions", this->power_subscriptions);
      }
#endif
   }

   return result;
}

int
ERSP::Unsubscribe(player_devaddr_t id)
{
   int result;

   // Do the unsubscription ...
   if ((result = Driver::Unsubscribe(id)) == 0) {
      // ... and decrement the appropriate subscription counter
      if (Device::MatchDeviceAddress(id, this->position_id)) {
         this->position_subscriptions--;
         assert(this->position_subscriptions >= 0);
         LOG_INFO("%d position subscriptions",
                this->position_subscriptions);
      }
      else if (Device::MatchDeviceAddress(id, this->bumper_id)) {
         this->bumper_subscriptions--;
         assert(this->bumper_subscriptions >= 0);
         LOG_INFO("%d bumper subscriptions",
                this->bumper_subscriptions);
      }
      else if (Device::MatchDeviceAddress(id, this->ir_range_id)) {
         this->ir_range_subscriptions--;
         assert(this->ir_range_subscriptions >= 0);
         LOG_INFO("%d ir subscriptions",
                this->ir_range_subscriptions);
      }
      else if (Device::MatchDeviceAddress(id, this->ir_binary_id)) {
         this->ir_binary_subscriptions--;
         assert(this->ir_binary_subscriptions >= 0);
         LOG_INFO("%d ir subscriptions",
                this->ir_binary_subscriptions);
      }
#ifdef POWER
      else if (Device::MatchDeviceAddress(id, this->power_id)) {
         this->power_subscriptions--;
         assert(this->power_subscriptions >= 0);
         LOG_INFO("%d power subscriptions", this->power_subscriptions);
      }
#endif
   }

   return result;
}

void
ERSP::PutData(void)
{
   LOG_INFO("Publishing odometry data");

   // FIXME: Use {position,bumper}_subscriptions to only
   // send conditionally on the number of subscribers.

   if (position_subscriptions)
   this->Publish(this->position_id, NULL,
              PLAYER_MSGTYPE_DATA,
              PLAYER_POSITION2D_DATA_STATE,
              (void*)&(this->ersp_data.position),
              sizeof(player_position2d_data_t),
              NULL);
 
   if (bumper_subscriptions)
   this->Publish(this->bumper_id, NULL,
              PLAYER_MSGTYPE_DATA,
              PLAYER_BUMPER_DATA_STATE,
              (void*)&(this->ersp_data.bumper),
              sizeof(player_bumper_data_t),
              NULL);
   
   if (ir_range_subscriptions)
   this->Publish(this->ir_range_id, NULL,
              PLAYER_MSGTYPE_DATA,
              PLAYER_IR_DATA_RANGES,
              (void*)&(this->ersp_data.ir_range),
              sizeof(player_ir_data_t),
              NULL);

   if (ir_binary_subscriptions)
   this->Publish(this->ir_binary_id, NULL,
              PLAYER_MSGTYPE_DATA,
              PLAYER_BUMPER_DATA_STATE,
              (void*)&(this->ersp_data.ir_binary),
              sizeof(player_bumper_data_t),
              NULL);

#ifdef POWER
   if (power_subscriptions)
   this->Publish(this->power_id, NULL,
              PLAYER_MSGTYPE_DATA,
              PLAYER_POWER_DATA_STATE,
              (void*)&(this->ersp_data.power),
              sizeof(player_power_data_t),
              NULL);
#endif   
}


// Driver module registration and initializaion

Driver*
ERSP_Init(ConfigFile* cf, int section)
{
   return (Driver*)(new ERSP(cf, section));
}

void ERSP_Register(DriverTable* table)
{
   table->AddDriver("ersp", ERSP_Init);
}

/* Need the extern to avoid C++ name-mangling  */
extern "C" {

int player_driver_init(DriverTable* table)
{
   LOG_INFO("Registering ERSP driver.");
   ERSP_Register(table);

   return 0;
}

}

---