Player

• Frontpage
• Contents

User

• Installation
• Quick start
• Supported devices
• Tutorials
• Utilities
• Client libraries
• FAQ
• Help

Developer

• Architecture
• libplayercore
  • interfaces
• libplayerdrivers
  • drivers
• libplayererror
• libplayerutils
• libplayersd
• libplayertcp
• libplayerxdr
• TODO

Online

Homepage
Download
Project
Bugs
Help

rflex.cc

/*  Player - One Hell of a Robot Server
 *  Copyright (C) 2000
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* notes:
 * the Main thread continues running when no-one is connected
 * this we retain our odometry data, whether anyone is connected or not
 */

/* Modified by Toby Collett, University of Auckland 2003-02-25
 * Added support for bump sensors for RWI b21r robot, uses DIO
 */

#include <fcntl.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>  /* for abs() */
#include <netinet/in.h>
#include <termios.h>
#include <assert.h>

#include "rflex.h"
#include "rflex_configs.h"

//rflex communications stuff
#include "rflex_commands.h"
#include "rflex-io.h"

#include <libplayercore/playercore.h>
#include <libplayerxdr/playerxdr.h>
extern PlayerTime* GlobalTime;

extern int               RFLEX::joy_control;

// help function to rotate sonar positions
void SonarRotate(double heading, double x1, double y1, double t1, double *x2, double *y2, double *t2)
{
  *t2 = t1 - heading;
  *x2 = x1*cos(heading) + y1*sin(heading);
  *y2 = -x1*sin(heading) + y1*cos(heading);
}


//NOTE - this is accessed as an extern variable by the other RFLEX objects
rflex_config_t rflex_configs;

/* initialize the driver.
 *
 * returns: pointer to new REBIR object
 */
Driver*
RFLEX_Init(ConfigFile *cf, int section)
{
  return (Driver *) new RFLEX( cf, section);
}

/* register the rflex driver in the drivertable
 *
 * returns:
 */
void
RFLEX_Register(DriverTable *table)
{
  table->AddDriver("rflex", RFLEX_Init);
}

// Message handler functions
void PrintHeader(player_msghdr_t hdr);
int RFLEX::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr,
                           void * data)
{
  assert(hdr);

  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_POWER,
                        sonar_id))
  {
    Lock();
    if(reinterpret_cast<player_sonar_power_config_t *> (data)->state==0)
      rflex_sonars_off(rflex_fd);
    else
      rflex_sonars_on(rflex_fd);
    Unlock();
    Publish(sonar_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_POWER,
                        sonar_id_2))
  {
    Lock();
    if(reinterpret_cast<player_sonar_power_config_t *> (data)->state==0)
      rflex_sonars_off(rflex_fd);
    else
      rflex_sonars_on(rflex_fd);
    Unlock();
    Publish(sonar_id_2, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_GET_GEOM,
                        sonar_id))
  {
    player_sonar_geom_t geom;
    Lock();
    geom.poses_count = rflex_configs.sonar_1st_bank_end;
    geom.poses = new player_pose3d_t[geom.poses_count];
    for (int i = 0; i < rflex_configs.sonar_1st_bank_end; i++)
    {
      geom.poses[i] = rflex_configs.mrad_sonar_poses[i];
    }
    Unlock();
    Publish(sonar_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    delete [] geom.poses;
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_GET_GEOM,
                        sonar_id_2))
  {
    player_sonar_geom_t geom;
    Lock();
    geom.poses_count = (rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start);
    geom.poses = new player_pose3d_t[geom.poses_count];
    for (int i = 0; i < rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start; i++)
    {
      geom.poses[i] = rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start];
    }
    Unlock();
    Publish(sonar_id_2, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    delete [] geom.poses;
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_BUMPER_REQ_GET_GEOM,
                        bumper_id))
  {
    player_bumper_geom_t geom;
    Lock();
    geom.bumper_def_count = rflex_configs.bumper_count;
    geom.bumper_def = new player_bumper_define_t[geom.bumper_def_count];
    for (int i = 0; i < rflex_configs.bumper_count; i++)
    {
      geom.bumper_def[i] = rflex_configs.bumper_def[i];
    }
    Unlock();
    Publish(bumper_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    delete [] geom.bumper_def;
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POSE, ir_id))
  {
    Lock();
    Publish(ir_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &rflex_configs.ir_poses,sizeof(rflex_configs.ir_poses));
    Unlock();
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POWER, ir_id))
  {
    player_ir_power_req_t * req = reinterpret_cast<player_ir_power_req_t*> (data);
    Lock();
    if (req->state == 0)
      rflex_ir_off(rflex_fd);
    else
      rflex_ir_on(rflex_fd);
    Unlock();
    Publish(ir_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, position_id))
  {
    player_position2d_set_odom_req * set_odom_req = reinterpret_cast<player_position2d_set_odom_req*> (data);
    Lock();
    set_odometry(set_odom_req->pose.px,set_odom_req->pose.py,set_odom_req->pose.pa);
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, position_id))
  {
    Lock();
    if(((player_position2d_power_config_t*)data)->state==0)
      rflex_brake_on(rflex_fd);
    else
      rflex_brake_off(rflex_fd);
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, position_id))
  {
    // Does nothing, needs to be implemented
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM, position_id))
  {
    Lock();
    reset_odometry();
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, position_id))
  {
    player_position2d_geom_t geom={{0}};
    Lock();
    geom.size.sl = rflex_configs.m_length;
    geom.size.sw = rflex_configs.m_width;
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, position_id))
  {
    Lock();
    command = *reinterpret_cast<player_position2d_cmd_vel_t *> (data);
    Unlock();
 
    // reset command_type since we have a new 
    // velocity command so we can get into the 
    // velocity control section in RFLEX::Main()
    
    command_type = 0;
    
    return 0;
  }

  return -1;
}

RFLEX::RFLEX(ConfigFile* cf, int section)
        : Driver(cf,section)
{
  // zero ids, so that we'll know later which interfaces were requested
  memset(&this->position_id, 0, sizeof(player_devaddr_t));
  memset(&this->sonar_id, 0, sizeof(player_devaddr_t));
  memset(&this->sonar_id_2, 0, sizeof(player_devaddr_t));
  memset(&this->ir_id, 0, sizeof(player_devaddr_t));
  memset(&this->bumper_id, 0, sizeof(player_devaddr_t));
  memset(&this->power_id, 0, sizeof(player_devaddr_t));
  memset(&this->aio_id, 0, sizeof(player_devaddr_t));
  memset(&this->dio_id, 0, sizeof(player_devaddr_t));
  
  command_type = 0;

  this->position_subscriptions = 0;
  this->sonar_subscriptions = 0;
  this->ir_subscriptions = 0;
  this->bumper_subscriptions = 0;

  // Do we create a robot position interface?
  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;
    }
  }
  else
  {
      PLAYER_WARN("Position2d interface not created for rflex driver");
  }

  // Do we create a sonar interface?
  if(cf->ReadDeviceAddr(&(this->sonar_id), section, "provides",
                      PLAYER_SONAR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->sonar_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("sonar bank 1 interface not created for rflex driver");
  }
  // Do we create a second sonar interface?
  if(cf->ReadDeviceAddr(&(this->sonar_id_2), section, "provides",
                      PLAYER_SONAR_CODE, -1, "bank2") == 0)
  {
    if(this->AddInterface(this->sonar_id_2) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("sonar bank 2 interface not created for rflex driver");
  }

  // Do we create an ir interface?
  if(cf->ReadDeviceAddr(&(this->ir_id), section, "provides",
                      PLAYER_IR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->ir_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("ir interface not created for rflex driver");
  }

  // Do we create a bumper interface?
  if(cf->ReadDeviceAddr(&(this->bumper_id), section, "provides",
                      PLAYER_BUMPER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->bumper_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("bumper interface not created for rflex driver");
  }

  // Do we create a power interface?
  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;
    }
  }
  else
  {
      PLAYER_WARN("power interface not created for rflex driver");
  }

  // Do we create an aio interface?
  if(cf->ReadDeviceAddr(&(this->aio_id), section, "provides",
                      PLAYER_AIO_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->aio_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("aio interface not created for rflex driver");
  }

  // Do we create a dio interface?
  if(cf->ReadDeviceAddr(&(this->dio_id), section, "provides",
                      PLAYER_DIO_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->dio_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("dio interface not created for rflex driver");
  }

  //just sets stuff to zero
  set_config_defaults();

  // joystick override
  joy_control = 0;

  //get serial port: everyone needs it (and we dont' want them fighting)
  strncpy(rflex_configs.serial_port,
          cf->ReadString(section, "rflex_serial_port",
                         rflex_configs.serial_port),
          sizeof(rflex_configs.serial_port));

  // Position-related options

  //length
  rflex_configs.m_length=
    cf->ReadFloat(section, "m_length",0.5);
  //width
  rflex_configs.m_width=
    cf->ReadFloat(section, "m_width",0.5);
  //distance conversion
  rflex_configs.odo_distance_conversion=
    cf->ReadFloat(section, "odo_distance_conversion", 0.0);
  //angle conversion
  rflex_configs.odo_angle_conversion=
    cf->ReadFloat(section, "odo_angle_conversion", 0.0);
  //default trans accel
  rflex_configs.mPsec2_trans_acceleration=
    cf->ReadFloat(section, "default_trans_acceleration",0.1);
  //default rot accel
  rflex_configs.radPsec2_rot_acceleration=
    cf->ReadFloat(section, "default_rot_acceleration",0.1);

  // absolute heading options
  rflex_configs.heading_home_address =
    cf->ReadInt(section, "rflex_heading_home_address",0);
  rflex_configs.home_on_start =
    cf->ReadInt(section, "rflex_home_on_start",0);

  // use rflex joystick for position
  rflex_configs.use_joystick |= cf->ReadInt(section, "rflex_joystick",0);
  rflex_configs.joy_pos_ratio = cf->ReadFloat(section, "rflex_joy_pos_ratio",0);
  rflex_configs.joy_ang_ratio = cf->ReadFloat(section, "rflex_joy_ang_ratio",0);

  // Sonar-related options
  int x;

  rflex_configs.range_distance_conversion=
          cf->ReadFloat(section, "range_distance_conversion",1476);
  rflex_configs.max_num_sonars=
          cf->ReadInt(section, "max_num_sonars",64);
  rflex_configs.num_sonars=
          cf->ReadInt(section, "num_sonars",24);
  rflex_configs.sonar_age=
          cf->ReadInt(section, "sonar_age",1);
  rflex_configs.sonar_max_range=
          cf->ReadInt(section, "sonar_max_range",3000);
  rflex_configs.num_sonar_banks=
          cf->ReadInt(section, "num_sonar_banks",8);
  rflex_configs.num_sonars_possible_per_bank=
          cf->ReadInt(section, "num_sonars_possible_per_bank",16);
  rflex_configs.num_sonars_in_bank=(int *) malloc(rflex_configs.num_sonar_banks*sizeof(double));
  for(x=0;x<rflex_configs.num_sonar_banks;x++)
    rflex_configs.num_sonars_in_bank[x]=
            (int) cf->ReadTupleFloat(section, "num_sonars_in_bank",x,8);
  rflex_configs.sonar_echo_delay=
          cf->ReadInt(section, "sonar_echo_delay",3000);
  rflex_configs.sonar_ping_delay=
          cf->ReadInt(section, "sonar_ping_delay",0);
  rflex_configs.sonar_set_delay=
          cf->ReadInt(section, "sonar_set_delay", 0);
  rflex_configs.mrad_sonar_poses=(player_pose3d_t *) malloc(rflex_configs.num_sonars*sizeof(player_pose3d_t));
  for(x=0;x<rflex_configs.num_sonars;x++)
  {
    rflex_configs.mrad_sonar_poses[x].px=
            cf->ReadTupleFloat(section, "mrad_sonar_poses",3*x+1,0.0);
    rflex_configs.mrad_sonar_poses[x].py=
            cf->ReadTupleFloat(section, "mrad_sonar_poses",3*x+2,0.0);
    rflex_configs.mrad_sonar_poses[x].pyaw=
            cf->ReadTupleFloat(section, "mrad_sonar_poses",3*x,0.0);
  }
  rflex_configs.sonar_2nd_bank_start=cf->ReadInt(section, "sonar_2nd_bank_start", 0);
  rflex_configs.sonar_1st_bank_end=rflex_configs.sonar_2nd_bank_start>0?rflex_configs.sonar_2nd_bank_start:rflex_configs.num_sonars;
  // IR-related options

  int pose_count=cf->ReadInt(section, "pose_count",8);
  rflex_configs.ir_base_bank=cf->ReadInt(section, "rflex_base_bank",0);
  rflex_configs.ir_bank_count=cf->ReadInt(section, "rflex_bank_count",0);
  rflex_configs.ir_min_range=cf->ReadFloat(section,"ir_min_range",0.1);
  rflex_configs.ir_max_range=cf->ReadFloat(section,"ir_max_range",0.8);
  rflex_configs.ir_count=new int[rflex_configs.ir_bank_count];
  rflex_configs.ir_a=new double[pose_count];
  rflex_configs.ir_b=new double[pose_count];
  rflex_configs.ir_poses.poses_count=pose_count;
  rflex_configs.ir_poses.poses=(player_pose3d_t *)
  malloc(rflex_configs.ir_poses.poses_count*sizeof(player_pose3d_t));
  unsigned int RunningTotal = 0;
  for(int i=0; i < rflex_configs.ir_bank_count; ++i)
    RunningTotal += (rflex_configs.ir_count[i]=(int) cf->ReadTupleFloat(section, "rflex_banks",i,0));
  rflex_configs.ir_total_count = RunningTotal;
  // posecount is actually unnecasary, but for consistancy will juse use it for error checking :)
  if (RunningTotal != rflex_configs.ir_poses.poses_count)
  {
    PLAYER_WARN("Error in config file, pose_count not equal to total poses in bank description\n");
    rflex_configs.ir_poses.poses_count = RunningTotal;
  }

  //  rflex_configs.ir_poses.poses=new int16_t[rflex_configs.ir_poses.pose_count];
  for(unsigned int i=0;i<rflex_configs.ir_poses.poses_count;i++)
  {
    rflex_configs.ir_poses.poses[i].px= cf->ReadTupleFloat(section, "poses",i*3,0);
    rflex_configs.ir_poses.poses[i].py= cf->ReadTupleFloat(section, "poses",i*3+1,0);
    rflex_configs.ir_poses.poses[i].pyaw= cf->ReadTupleFloat(section, "poses",i*3+2,0);

    // Calibration parameters for ir in form range=(a*voltage)^b
    rflex_configs.ir_a[i] = cf->ReadTupleFloat(section, "rflex_ir_calib",i*2,1);
    rflex_configs.ir_b[i] = cf->ReadTupleFloat(section, "rflex_ir_calib",i*2+1,1);
  }

  // Bumper-related options
  rflex_configs.bumper_count = cf->ReadInt(section, "bumper_count",0);
  rflex_configs.bumper_def = new player_bumper_define_t[rflex_configs.bumper_count];
  for(x=0;x<rflex_configs.bumper_count;++x)
  {
    rflex_configs.bumper_def[x].pose.px =  (cf->ReadTupleFloat(section, "bumper_def",5*x,0)); //m
    rflex_configs.bumper_def[x].pose.py =  (cf->ReadTupleFloat(section, "bumper_def",5*x+1,0)); //m
    rflex_configs.bumper_def[x].pose.pyaw =  (cf->ReadTupleFloat(section, "bumper_def",5*x+2,0)); //rad
    rflex_configs.bumper_def[x].length =  (cf->ReadTupleFloat(section, "bumper_def",5*x+3,0)); //m
    rflex_configs.bumper_def[x].radius =  (cf->ReadTupleFloat(section, "bumper_def",5*x+4,0));  //m
  }
  rflex_configs.bumper_address = cf->ReadInt(section, "rflex_bumper_address",DEFAULT_RFLEX_BUMPER_ADDRESS);


  const char *bumperStyleStr = cf->ReadString(section, "rflex_bumper_style",DEFAULT_RFLEX_BUMPER_STYLE);

  if(strcmp(bumperStyleStr,RFLEX_BUMPER_STYLE_BIT) == 0)
  {
    rflex_configs.bumper_style = BUMPER_BIT;
  }
  else if(strcmp(bumperStyleStr,RFLEX_BUMPER_STYLE_ADDR) == 0)
  {
    rflex_configs.bumper_style = BUMPER_ADDR;
  }
  else
  {
    //Invalid value
    rflex_configs.bumper_style = BUMPER_ADDR;
  }

  // Power-related options
  rflex_configs.power_offset = cf->ReadFloat(section, "rflex_power_offset",DEFAULT_RFLEX_POWER_OFFSET);

  rflex_fd = -1;

}

RFLEX::~RFLEX()
{
}

int RFLEX::Setup(){
  /* now spawn reading thread */
  StartThread();
  return(0);
}

int RFLEX::Shutdown()
{
  if(rflex_fd == -1)
  {
    return 0;
  }
  StopThread();
  //make sure it doesn't go anywhere
  rflex_stop_robot(rflex_fd,(int) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration));
  //kill that infernal clicking
  rflex_sonars_off(rflex_fd);
  // release the port
  rflex_close_connection(&rflex_fd);
  return 0;
}

/* start a thread that will invoke Main() */
void
RFLEX::StartThread(void)
{
  ThreadAlive = true;
  pthread_create(&driverthread, NULL, &DummyMain, this);
}

/* wait for termination of the thread */
// this is called with the lock held
void
RFLEX::StopThread(void)
{
  void* dummy;
  ThreadAlive = false;
  Unlock();

  //pthread_cancel(driverthread);
  if(pthread_join(driverthread,&dummy))
    perror("Driver::StopThread:pthread_join()");
  Lock();
}

int
RFLEX::Subscribe(player_devaddr_t id)
{
  int setupResult;

  // do the subscription
  if((setupResult = Driver::Subscribe(id)) == 0)
  {
    // also increment the appropriate subscription counter
    switch(id.interf)
    {
      case PLAYER_POSITION2D_CODE:
        this->position_subscriptions++;
        break;
      case PLAYER_SONAR_CODE:
        this->sonar_subscriptions++;
        break;
      case PLAYER_BUMPER_CODE:
        this->bumper_subscriptions++;
        break;
      case PLAYER_IR_CODE:
        this->ir_subscriptions++;
        break;
    }
  }

  return(setupResult);
}

int
RFLEX::Unsubscribe(player_devaddr_t id)
{
  int shutdownResult;

  // do the unsubscription
  if((shutdownResult = Driver::Unsubscribe(id)) == 0)
  {
    // also decrement the appropriate subscription counter
    switch(id.interf)
    {
      case PLAYER_POSITION2D_CODE:
        --this->position_subscriptions;
        assert(this->position_subscriptions >= 0);
        break;
      case PLAYER_SONAR_CODE:
        sonar_subscriptions--;
        assert(sonar_subscriptions >= 0);
        break;
      case PLAYER_BUMPER_CODE:
        --this->bumper_subscriptions;
        assert(this->bumper_subscriptions >= 0);
        break;
      case PLAYER_IR_CODE:
        --this->ir_subscriptions;
        assert(this->ir_subscriptions >= 0);
        break;
    }
  }

  return(shutdownResult);
}

void
RFLEX::Main()
{
  PLAYER_MSG1(1,"%s","Rflex Thread Started");

  //sets up connection, and sets defaults
  //configures sonar, motor acceleration, etc.
  if(initialize_robot()<0){
    PLAYER_ERROR("ERROR, no connection to RFLEX established\n");
    return;
  }
  Lock();
  reset_odometry();
  Unlock();



  static double mPsec_speedDemand=0.0, radPsec_turnRateDemand=0.0;
  bool newmotorspeed, newmotorturn;

  int i;
  int last_sonar_subscrcount = 0;
  int last_position_subscrcount = 0;
  int last_ir_subscrcount = 0;

  while(1)
  {
    int oldstate;
    int ret;
    ret = pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,&oldstate);

    // we want to turn on the sonars if someone just subscribed, and turn
    // them off if the last subscriber just unsubscribed.
    if(!last_sonar_subscrcount && this->sonar_subscriptions)
    {
      Lock();
        rflex_sonars_on(rflex_fd);
        Unlock();
    }
    else if(last_sonar_subscrcount && !(this->sonar_subscriptions))
    {
      Lock();
    rflex_sonars_off(rflex_fd);
    Unlock();
    }
    last_sonar_subscrcount = this->sonar_subscriptions;

    // we want to turn on the ir if someone just subscribed, and turn
    // it off if the last subscriber just unsubscribed.
    if(!last_ir_subscrcount && this->ir_subscriptions)
    {
      Lock();
    rflex_ir_on(rflex_fd);
    Unlock();
    }
    else if(last_ir_subscrcount && !(this->ir_subscriptions))
    {
      Lock();
    rflex_ir_off(rflex_fd);
    Unlock();
    }
    last_ir_subscrcount = this->ir_subscriptions;

    // we want to reset the odometry and enable the motors if the first
    // client just subscribed to the position device, and we want to stop
    // and disable the motors if the last client unsubscribed.

    //first user logged in
    if(!last_position_subscrcount && this->position_subscriptions)
    {
      Lock();
      //set drive defaults
      rflex_motion_set_defaults(rflex_fd);

      //make sure robot doesn't go anywhere
      rflex_stop_robot(rflex_fd,(int) (M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration)));

    Unlock();
    }
    //last user logged out
    else if(last_position_subscrcount && !(this->position_subscriptions))
    {
      Lock();
      //make sure robot doesn't go anywhere
      rflex_stop_robot(rflex_fd,(int) (M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration)));
      // disable motor power
      rflex_brake_on(rflex_fd);
      Unlock();
    }
    last_position_subscrcount = this->position_subscriptions;

  ProcessMessages();

    if(this->position_subscriptions || rflex_configs.use_joystick || rflex_configs.home_on_start)
    {
    Lock();
    newmotorspeed = false;
    newmotorturn = false;

    if (rflex_configs.home_on_start)
    {
      command.vel.pa = M_PI/18;
      newmotorturn=true;
    }

    if(mPsec_speedDemand != command.vel.px)
    {
          newmotorspeed = true;
          mPsec_speedDemand = command.vel.px;
        }
        if(radPsec_turnRateDemand != command.vel.pa)
        {
          newmotorturn = true;
          radPsec_turnRateDemand = command.vel.pa;
        }
        /* NEXT, write commands */
        // rflex has a built in failsafe mode where if no move command is recieved in a
        // certain interval the robot stops.
        // this is a good thing given teh size of the robot...
        // if network goes down or some such and the user looses control then the robot stops
        // if the robot is running in an autonomous mdoe it is easy enough to simply
        // resend the command repeatedly

        // allow rflex joystick to overide the player command
        if (joy_control > 0)
          --joy_control;
        // only set new command if type is valid and their is a new command
        else if (command_type == 0)
        {
          rflex_set_velocity(rflex_fd,(long) M2ARB_ODO_CONV(mPsec_speedDemand),(long) RAD2ARB_ODO_CONV(radPsec_turnRateDemand),(long) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration));
          command_type = 255;
        }
        Unlock();
    }
    else
    {
      Lock();
    rflex_stop_robot(rflex_fd,(long) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration));
    Unlock();
    }

    /* Get data from robot */
  static float LastYaw = 0;
    player_rflex_data_t rflex_data = {{{0}}};

  Lock();
    update_everything(&rflex_data);
    Unlock();

  if (position_id.interf != 0)
  {
    Publish(this->position_id,PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_STATE,
            (unsigned char*)&rflex_data.position,
            sizeof(player_position2d_data_t),
            NULL);
  }
  if (sonar_id.interf != 0)
  {
      Publish(this->sonar_id,PLAYER_MSGTYPE_DATA,PLAYER_SONAR_DATA_RANGES,
            (unsigned char*)&rflex_data.sonar,
            sizeof(player_sonar_data_t),
            NULL);
  }
  // Here we check if the robot has changed Yaw...
  // If it has we need to update the geometry as well
  if (rflex_data.position.pos.pa != LastYaw)
  {
    // Transmit new sonar geometry
    double NewGeom[3];

        player_sonar_geom_t geom;

    Lock();
        geom.poses_count = rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start;
        geom.poses = new player_pose3d_t[geom.poses_count];
        for (i = 0; i < rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start; i++)
        {
      SonarRotate(rad_odo_theta, rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start].px,rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start].py,rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start].pyaw,NewGeom,&NewGeom[1],&NewGeom[2]);
          geom.poses[i].px = NewGeom[0];
          geom.poses[i].py = NewGeom[1];
          geom.poses[i].pyaw = NewGeom[2];
        }
        Unlock();
        if (sonar_id_2.interf)
        Publish(this->sonar_id_2,  PLAYER_MSGTYPE_DATA, PLAYER_SONAR_DATA_GEOM,
        (unsigned char*)&geom, sizeof(player_sonar_geom_t), NULL);
        delete [] geom.poses;
  }
  LastYaw = rflex_data.position.pos.pa;

  if (sonar_id_2.interf)
      Publish(this->sonar_id_2,PLAYER_MSGTYPE_DATA,PLAYER_SONAR_DATA_RANGES,
            (unsigned char*)&rflex_data.sonar2,
            sizeof(player_sonar_data_t),
            NULL);
  if (ir_id.interf)
      Publish(this->ir_id,PLAYER_MSGTYPE_DATA,PLAYER_IR_DATA_RANGES,
            (unsigned char*)&rflex_data.ir,
            sizeof(player_ir_data_t),
            NULL);
  if (bumper_id.interf)
      Publish(this->bumper_id,PLAYER_MSGTYPE_DATA,PLAYER_BUMPER_DATA_STATE,
            (unsigned char*)&rflex_data.bumper,
            sizeof(player_bumper_data_t),
            NULL);
  if (power_id.interf)
    Publish(this->power_id,PLAYER_MSGTYPE_DATA,PLAYER_POWER_DATA_STATE,
            (unsigned char*)&rflex_data.power,
            sizeof(player_power_data_t),
            NULL);
  if (aio_id.interf != 0)
  {
      Publish(this->aio_id,PLAYER_MSGTYPE_DATA,PLAYER_AIO_DATA_STATE,
            (unsigned char*)&rflex_data.aio,
            sizeof(player_aio_data_t),
            NULL);
  }
  if (dio_id.interf != 0)
  {
      Publish(this->dio_id,PLAYER_MSGTYPE_DATA,PLAYER_DIO_DATA_VALUES,
            (unsigned char*)&rflex_data.dio,
            sizeof(player_dio_data_t),
            NULL);
  }

  ret=pthread_setcancelstate(oldstate,NULL);
  player_position2d_data_t_cleanup(&rflex_data.position);
  player_sonar_data_t_cleanup(&rflex_data.sonar);
  player_sonar_data_t_cleanup(&rflex_data.sonar2);
  player_gripper_data_t_cleanup(&rflex_data.gripper);
  player_power_data_t_cleanup(&rflex_data.power);
  player_bumper_data_t_cleanup(&rflex_data.bumper);
  player_dio_data_t_cleanup(&rflex_data.dio);
  player_aio_data_t_cleanup(&rflex_data.aio);
  player_ir_data_t_cleanup(&rflex_data.ir);
  Lock();
  if (!ThreadAlive)
  {
    Unlock();
    break;
  }
  Unlock();

  // release cpu somewhat so other threads can run.
  usleep(1000);
  
  }
  pthread_exit(NULL);
}

int RFLEX::initialize_robot(){
  // Neither g++ nor I can find a definition for thread_is_running - BPG
#if 0
#ifdef _REENTRANT
  if (thread_is_running)
    {
      fprintf(stderr,"Tried to connect to the robot while the command loop "
                  "thread is running.\n");
      fprintf(stderr,"This is a bug in the code, and must be fixed.\n");
      return -1;
    }
#endif
#endif

  if (rflex_open_connection(rflex_configs.serial_port, &rflex_fd) < 0)
    return -1;

  printf("Rflex initialisation called\n");
  rflex_initialize(rflex_fd, (int) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration),(int) RAD2ARB_ODO_CONV(rflex_configs.radPsec2_rot_acceleration), 0, 0);
  printf("RFlex init done\n");

  return 0;
}

void RFLEX::reset_odometry() {
  m_odo_x=0;
  m_odo_y=0;
  rad_odo_theta= 0.0;
}

void RFLEX::set_odometry(float m_x, float m_y, float rad_theta) {
  m_odo_x=m_x;
  m_odo_y=m_y;
  rad_odo_theta=rad_theta;
}

void RFLEX::update_everything(player_rflex_data_t* d)
{

  int *arb_ranges = new int[rflex_configs.num_sonars];
  char *abumper_ranges = new char[rflex_configs.bumper_count];
  uint8_t *air_ranges = new uint8_t[rflex_configs.ir_total_count];

  static int initialized = 0;

  double m_new_range_position; double rad_new_bearing_position;
  double mPsec_t_vel;
  double radPsec_r_vel;

  int arb_t_vel, arb_r_vel;
  static int arb_last_range_position;
  static int arb_last_bearing_position;
  int arb_new_range_position;
  int arb_new_bearing_position;
  double m_displacement;
  short a_num_sonars, a_num_bumpers, a_num_ir;

  int batt,brake;

  int i;

  //update status
  rflex_update_status(rflex_fd, &arb_new_range_position,
                      &arb_new_bearing_position, &arb_t_vel,
                      &arb_r_vel);
  mPsec_t_vel=ARB2M_ODO_CONV(arb_t_vel);
  radPsec_r_vel=ARB2RAD_ODO_CONV(arb_r_vel);
  m_new_range_position=ARB2M_ODO_CONV(arb_new_range_position);
  rad_new_bearing_position=ARB2RAD_ODO_CONV(arb_new_bearing_position);

  if (!initialized) {
    initialized = 1;
  } else {
    rad_odo_theta += ARB2RAD_ODO_CONV(arb_new_bearing_position - arb_last_bearing_position);
    rad_odo_theta = normalize_theta(rad_odo_theta);
    m_displacement = ARB2M_ODO_CONV(arb_new_range_position - arb_last_range_position);

    //integrate latest motion into odometry
    m_odo_x += m_displacement * cos(rad_odo_theta);
    m_odo_y += m_displacement * sin(rad_odo_theta);
    d->position.pos.px = m_odo_x;
    d->position.pos.py = m_odo_y;
    while(rad_odo_theta<0)
      rad_odo_theta+=2*M_PI;
    while(rad_odo_theta>2*M_PI)
      rad_odo_theta-=2*M_PI;
    d->position.pos.pa = rad_odo_theta;

    d->position.vel.px = mPsec_t_vel;
    d->position.vel.pa = radPsec_r_vel;
    //TODO - get better stall information (battery draw?)
  }
  d->position.stall = false;

  arb_last_range_position = arb_new_range_position;
  arb_last_bearing_position = arb_new_bearing_position;

   //note - sonar mappings are strange - look in rflex_commands.c
  if(this->sonar_subscriptions)
  {
    // TODO - currently bad sonar data is sent back to clients
    // (not enough data buffered, so sonar sent in wrong order - missing intermittent sonar values - fix this
    a_num_sonars=rflex_configs.num_sonars;

//    pthread_testcancel();
    rflex_update_sonar(rflex_fd, a_num_sonars,
           arb_ranges);
//    pthread_testcancel();
    if (d->sonar.ranges_count!=rflex_configs.sonar_1st_bank_end)
    {
      d->sonar.ranges_count=rflex_configs.sonar_1st_bank_end;
      delete [] d->sonar.ranges;
      d->sonar.ranges = new float[d->sonar.ranges_count];
    }
    for (i = 0; i < rflex_configs.sonar_1st_bank_end; i++){
      d->sonar.ranges[i] = ARB2M_RANGE_CONV(arb_ranges[i]);
    }
    if (d->sonar2.ranges_count!=(rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start))
    {
      d->sonar2.ranges_count=(rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start);
      delete [] d->sonar2.ranges;
      d->sonar2.ranges = new float[d->sonar2.ranges_count];
    }
    for (i = 0; i < rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start; i++){
      d->sonar2.ranges[i] = ARB2M_RANGE_CONV(arb_ranges[rflex_configs.sonar_2nd_bank_start+i]);
    }
  }

  // if someone is subscribed to bumpers copy internal data to device
  if(this->bumper_subscriptions)
  {
    a_num_bumpers=rflex_configs.bumper_count;

//    pthread_testcancel();
    // first make sure our internal state is up to date
    rflex_update_bumpers(rflex_fd, a_num_bumpers, abumper_ranges);
 //   pthread_testcancel();

    d->bumper.bumpers_count=(a_num_bumpers);
    d->bumper.bumpers = (uint8_t*)abumper_ranges;
  }

  // if someone is subscribed to irs copy internal data to device
  if(this->ir_subscriptions)
  {
    a_num_ir=rflex_configs.ir_poses.poses_count;

//    pthread_testcancel();
    // first make sure our internal state is up to date
    rflex_update_ir(rflex_fd, a_num_ir, air_ranges);
//    pthread_testcancel();

    if (d->ir.ranges_count != (unsigned int) a_num_ir)
    {
      d->ir.ranges_count = a_num_ir;
      d->ir.voltages_count = a_num_ir;
      delete [] d->ir.ranges;
      delete [] d->ir.voltages;
      d->ir.ranges = new float [a_num_ir];
      d->ir.voltages = new float [a_num_ir];
    }
    for (int i = 0; i < a_num_ir; ++i)
    {
      d->ir.voltages[i] = air_ranges[i];
      // using power law mapping of form range = a*voltage^b
      float range = pow(air_ranges[i],rflex_configs.ir_b[i]) * rflex_configs.ir_a[i];
      // check for min and max ranges, < min = 0 > max = max
      range = range < rflex_configs.ir_min_range ? 0 : range;
      range = range > rflex_configs.ir_max_range ? rflex_configs.ir_max_range : range;
      d->ir.ranges[i] = (range);

    }
  }

  //this would get the battery,time, and brake state (if we cared)
  //update system (battery,time, and brake also request joystick data)
  rflex_update_system(rflex_fd,&batt,&brake);
  
  // set the bits for the fields we're using
  d->power.valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
  
  d->power.volts = static_cast<float>(batt)/100.0 + rflex_configs.power_offset;
  if (d->power.volts > 24)
    d->power.percent = 100;
  else if (d->power.volts < 20)
    d->power.percent = 0;
  else
    d->power.percent = 100.0*(d->power.volts-20.0)/4.0;
}

//this is so things don't crash if we don't load a device
//(and thus it's settings)
void RFLEX::set_config_defaults(){
  memset(&rflex_configs, 0, sizeof(rflex_configs));
  strcpy(rflex_configs.serial_port,"/dev/ttyR0");
  rflex_configs.mPsec2_trans_acceleration=0.500;
  rflex_configs.radPsec2_rot_acceleration=0.500;
}

---

Last updated 12 September 2005 21:38:45