p2os.cpp

Go to the documentation of this file.

/*
 *  P2OS for ROS
 *  Copyright (C) 2009  David Feil-Seifer, Brian Gerkey, Kasper Stoy,
 *     Richard Vaughan, & Andrew Howard
 *  Copyright (C) 2018  Hunter L. Allen
 *
 *  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
 *
 */
#include <termios.h>
#include <fcntl.h>
#include <string.h>

#include <ros/ros.h>
#include <p2os_driver/p2os.hpp>

#include <string>

P2OSNode::P2OSNode(ros::NodeHandle nh)
: n(nh),
  batt_pub_(n.advertise<p2os_msgs::BatteryState>("battery_state", 1000),
    diagnostic_,
    diagnostic_updater::FrequencyStatusParam(&desired_freq, &desired_freq, 0.1),
    diagnostic_updater::TimeStampStatusParam()),
  ptz_(this)
{
  // Use sonar
  ros::NodeHandle n_private("~");
  n_private.param(std::string("odom_frame_id"), odom_frame_id, std::string("odom"));
  n_private.param(std::string("base_link_frame_id"), base_link_frame_id, std::string("base_link"));
  n_private.param("use_sonar", use_sonar_, false);

  // read in config options
  // bumpstall
  n_private.param("bumpstall", bumpstall, -1);
  // pulse
  n_private.param("pulse", pulse, -1.0);
  // rot_kp
  n_private.param("rot_kp", rot_kp, -1);
  // rot_kv
  n_private.param("rot_kv", rot_kv, -1);
  // rot_ki
  n_private.param("rot_ki", rot_ki, -1);
  // trans_kp
  n_private.param("trans_kp", trans_kp, -1);
  // trans_kv
  n_private.param("trans_kv", trans_kv, -1);
  // trans_ki
  n_private.param("trans_ki", trans_ki, -1);
  // !!! port !!!
  std::string def = DEFAULT_P2OS_PORT;
  n_private.param("port", psos_serial_port, def);
  ROS_INFO("using serial port: [%s]", psos_serial_port.c_str());
  n_private.param("use_tcp", psos_use_tcp, false);
  std::string host = DEFAULT_P2OS_TCP_REMOTE_HOST;
  n_private.param("tcp_remote_host", psos_tcp_host, host);
  n_private.param("tcp_remote_port", psos_tcp_port, DEFAULT_P2OS_TCP_REMOTE_PORT);
  // radio
  n_private.param("radio", radio_modemp, 0);
  // joystick
  n_private.param("joystick", joystick, 0);
  // direct_wheel_vel_control
  n_private.param("direct_wheel_vel_control", direct_wheel_vel_control, 0);
  // max xpeed
  double spd;
  n_private.param("max_xspeed", spd, MOTOR_DEF_MAX_SPEED);
  motor_max_speed = static_cast<int>(rint(1e3 * spd));
  // max_yawspeed
  n_private.param("max_yawspeed", spd, MOTOR_DEF_MAX_TURNSPEED);
  motor_max_turnspeed = static_cast<int16_t>(rint(RTOD(spd)));
  // max_xaccel
  n_private.param("max_xaccel", spd, 0.0);
  motor_max_trans_accel = static_cast<int16_t>(rint(1e3 * spd));
  // max_xdecel
  n_private.param("max_xdecel", spd, 0.0);
  motor_max_trans_decel = static_cast<int16_t>(rint(1e3 * spd));
  // max_yawaccel
  n_private.param("max_yawaccel", spd, 0.0);
  motor_max_rot_accel = static_cast<int16_t>(rint(RTOD(spd)));
  // max_yawdecel
  n_private.param("max_yawdecel", spd, 0.0);
  motor_max_rot_decel = static_cast<int16_t>(rint(RTOD(spd)));

  desired_freq = 10;

  // advertise services
  pose_pub_ = n.advertise<nav_msgs::Odometry>("pose", 1000);
  // advertise topics
  mstate_pub_ = n.advertise<p2os_msgs::MotorState>("motor_state", 1000);
  grip_state_pub_ = n.advertise<p2os_msgs::GripperState>("gripper_state", 1000);
  ptz_state_pub_ = n.advertise<p2os_msgs::PTZState>("ptz_state", 1000);
  sonar_pub_ = n.advertise<p2os_msgs::SonarArray>("sonar", 1000);
  aio_pub_ = n.advertise<p2os_msgs::AIO>("aio", 1000);
  dio_pub_ = n.advertise<p2os_msgs::DIO>("dio", 1000);

  // subscribe to services
  cmdvel_sub_ = n.subscribe("cmd_vel", 1, &P2OSNode::cmdvel_cb, this);
  cmdmstate_sub_ = n.subscribe("cmd_motor_state", 1, &P2OSNode::cmdmotor_state,
      this);
  gripper_sub_ = n.subscribe("gripper_control", 1, &P2OSNode::gripperCallback,
      this);
  ptz_cmd_sub_ = n.subscribe("ptz_control", 1, &P2OSPtz::callback, &ptz_);

  veltime = ros::Time::now();

  // add diagnostic functions
  diagnostic_.add("Motor Stall", this, &P2OSNode::check_stall);
  diagnostic_.add("Battery Voltage", this, &P2OSNode::check_voltage);

  // initialize robot parameters (player legacy)
  initialize_robot_params();
}

P2OSNode::~P2OSNode() { }

void P2OSNode::cmdmotor_state(const p2os_msgs::MotorStateConstPtr & msg)
{
  motor_dirty = true;
  cmdmotor_state_ = *msg;
}

void P2OSNode::check_and_set_motor_state()
{
  if (!motor_dirty) {
    return;
  }
  motor_dirty = false;

  unsigned char val = static_cast<unsigned char>(cmdmotor_state_.state);
  unsigned char command[4];
  P2OSPacket packet;
  command[0] = ENABLE;
  command[1] = ARGINT;
  command[2] = val;
  command[3] = 0;
  packet.Build(command, 4);

  // Store the current motor state so that we can set it back
  p2os_data.motors.state = cmdmotor_state_.state;
  SendReceive(&packet, false);
}

void P2OSNode::check_and_set_gripper_state()
{
  if (!gripper_dirty_) {
    return;
  }
  gripper_dirty_ = false;

  // Send the gripper command
  unsigned char grip_val = (unsigned char) gripper_state_.grip.state;
  unsigned char grip_command[4];

  P2OSPacket grip_packet;
  grip_command[0] = GRIPPER;
  grip_command[1] = ARGINT;
  grip_command[2] = grip_val;
  grip_command[3] = 0;
  grip_packet.Build(grip_command, 4);
  SendReceive(&grip_packet, false);

  // Send the lift command
  unsigned char lift_val = (unsigned char) gripper_state_.lift.state;
  unsigned char lift_command[4];

  P2OSPacket lift_packet;
  lift_command[0] = GRIPPER;
  lift_command[1] = ARGINT;
  lift_command[2] = lift_val;
  lift_command[3] = 0;
  lift_packet.Build(lift_command, 4);

  SendReceive(&lift_packet, false);
}

void P2OSNode::cmdvel_cb(const geometry_msgs::TwistConstPtr & msg)
{
  if (fabs(msg->linear.x - cmdvel_.linear.x) > 0.01 ||
    fabs(msg->angular.z - cmdvel_.angular.z) > 0.01)
  {
    veltime = ros::Time::now();
    ROS_DEBUG("new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x * 1e3, msg->angular.z,
      veltime.toSec());
    vel_dirty = true;
    cmdvel_ = *msg;
  } else {
    ros::Duration veldur = ros::Time::now() - veltime;
    if (veldur.toSec() > 2.0 &&
      ((fabs(cmdvel_.linear.x) > 0.01) || (fabs(cmdvel_.angular.z) > 0.01)))
    {
      ROS_DEBUG("maintaining old speed: %0.3f|%0.3f", veltime.toSec(), ros::Time::now().toSec());
      vel_dirty = true;
      veltime = ros::Time::now();
    }
  }
}

void P2OSNode::check_and_set_vel()
{
  if (!vel_dirty) {return;}

  ROS_DEBUG("setting vel: [%0.2f,%0.2f]", cmdvel_.linear.x, cmdvel_.angular.z);
  vel_dirty = false;

  uint16_t absSpeedDemand, absturnRateDemand;
  unsigned char motorcommand[4];
  P2OSPacket motorpacket;

  int vx = static_cast<int>(cmdvel_.linear.x * 1e3);
  int va = static_cast<int>(rint(RTOD(cmdvel_.angular.z)));

  // non-direct wheel control
  motorcommand[0] = VEL;
  motorcommand[1] = (vx >= 0) ? ARGINT : ARGNINT;

  absSpeedDemand = static_cast<uint16_t>(abs(vx));

  if (absSpeedDemand <= this->motor_max_speed) {
    motorcommand[2] = absSpeedDemand & 0x00FF;
    motorcommand[3] = (absSpeedDemand & 0xFF00) >> 8;
  } else {
    ROS_WARN("speed demand thresholded! (true: %u, max: %u)", absSpeedDemand, motor_max_speed);
    motorcommand[2] = motor_max_speed & 0x00FF;
    motorcommand[3] = (motor_max_speed & 0xFF00) >> 8;
  }

  motorpacket.Build(motorcommand, 4);
  SendReceive(&motorpacket);

  motorcommand[0] = RVEL;
  motorcommand[1] = (va >= 0) ? ARGINT : ARGNINT;

  absturnRateDemand = static_cast<uint16_t>((va >= 0) ? va : (-1 * va));

  if (absturnRateDemand <= motor_max_turnspeed) {
    motorcommand[2] = absturnRateDemand & 0x00FF;
    motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8;
  } else {
    ROS_WARN("Turn rate demand threshholded!");
    motorcommand[2] = this->motor_max_turnspeed & 0x00FF;
    motorcommand[3] = (this->motor_max_turnspeed & 0xFF00) >> 8;
  }

  motorpacket.Build(motorcommand, 4);
  SendReceive(&motorpacket);
}

void P2OSNode::gripperCallback(const p2os_msgs::GripperStateConstPtr & msg)
{
  gripper_dirty_ = true;
  gripper_state_ = *msg;
}

int P2OSNode::Setup()
{
  int i;
  int bauds[] = {B9600, B38400, B19200, B115200, B57600};
  int numbauds = sizeof(bauds);
  int currbaud = 0;
  sippacket = NULL;
  lastPulseTime = 0.0;

  struct termios term;
  unsigned char command;
  P2OSPacket packet, receivedpacket;
  int flags = 0;
  bool sent_close = false;

  enum
  {
    NO_SYNC,
    AFTER_FIRST_SYNC,
    AFTER_SECOND_SYNC,
    READY
  } psos_state;

  psos_state = NO_SYNC;

  char name[20], type[20], subtype[20];
  int cnt;


  // use serial port

  ROS_INFO("P2OS connection opening serial port %s...", psos_serial_port.c_str());

  if ((this->psos_fd = open(this->psos_serial_port.c_str(),
    O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR)) < 0)
  {
    ROS_ERROR("P2OS::Setup():open():");
    return 1;
  }

  if (tcgetattr(this->psos_fd, &term) < 0) {
    ROS_ERROR("P2OS::Setup():tcgetattr():");
    close(this->psos_fd);
    this->psos_fd = -1;
    return 1;
  }

  cfmakeraw(&term);
  cfsetispeed(&term, bauds[currbaud]);
  cfsetospeed(&term, bauds[currbaud]);

  if (tcsetattr(this->psos_fd, TCSAFLUSH, &term) < 0) {
    ROS_ERROR("P2OS::Setup():tcsetattr():");
    close(this->psos_fd);
    this->psos_fd = -1;
    return 1;
  }

  if (tcflush(this->psos_fd, TCIOFLUSH) < 0) {
    ROS_ERROR("P2OS::Setup():tcflush():");
    close(this->psos_fd);
    this->psos_fd = -1;
    return 1;
  }

  if ((flags = fcntl(this->psos_fd, F_GETFL)) < 0) {
    ROS_ERROR("P2OS::Setup():fcntl()");
    close(this->psos_fd);
    this->psos_fd = -1;
    return 1;
  }
  // Sync:

  int num_sync_attempts = 3;
  while (psos_state != READY) {
    switch (psos_state) {
      case NO_SYNC:
        command = SYNC0;
        packet.Build(&command, 1);
        packet.Send(this->psos_fd);
        usleep(P2OS_CYCLETIME_USEC);
        break;
      case AFTER_FIRST_SYNC:
        ROS_INFO("turning off NONBLOCK mode...");
        if (fcntl(this->psos_fd, F_SETFL, flags ^ O_NONBLOCK) < 0) {
          ROS_ERROR("P2OS::Setup():fcntl()");
          close(this->psos_fd);
          this->psos_fd = -1;
          return 1;
        }
        command = SYNC1;
        packet.Build(&command, 1);
        packet.Send(this->psos_fd);
        break;
      case AFTER_SECOND_SYNC:
        command = SYNC2;
        packet.Build(&command, 1);
        packet.Send(this->psos_fd);
        break;
      default:
        ROS_WARN("P2OS::Setup():shouldn't be here...");
        break;
    }
    usleep(P2OS_CYCLETIME_USEC);

    if (receivedpacket.Receive(this->psos_fd)) {
      if ((psos_state == NO_SYNC) && (num_sync_attempts >= 0)) {
        num_sync_attempts--;
        usleep(P2OS_CYCLETIME_USEC);
        continue;
      } else {
        // couldn't connect; try different speed.
        if (++currbaud < numbauds) {
          cfsetispeed(&term, bauds[currbaud]);
          cfsetospeed(&term, bauds[currbaud]);
          if (tcsetattr(this->psos_fd, TCSAFLUSH, &term) < 0) {
            ROS_ERROR("P2OS::Setup():tcsetattr():");
            close(this->psos_fd);
            this->psos_fd = -1;
            return 1;
          }

          if (tcflush(this->psos_fd, TCIOFLUSH) < 0) {
            ROS_ERROR("P2OS::Setup():tcflush():");
            close(this->psos_fd);
            this->psos_fd = -1;
            return 1;
          }
          num_sync_attempts = 3;
          continue;
        } else {
          // tried all speeds; bail
          break;
        }
      }
    }
    switch (receivedpacket.packet[3]) {
      case SYNC0:
        ROS_INFO("SYNC0");
        psos_state = AFTER_FIRST_SYNC;
        break;
      case SYNC1:
        ROS_INFO("SYNC1");
        psos_state = AFTER_SECOND_SYNC;
        break;
      case SYNC2:
        ROS_INFO("SYNC2");
        psos_state = READY;
        break;
      default:
        // maybe P2OS is still running from last time.  let's try to CLOSE
        // and reconnect
        if (!sent_close) {
          ROS_DEBUG("sending CLOSE");
          command = CLOSE;
          packet.Build(&command, 1);
          packet.Send(this->psos_fd);
          sent_close = true;
          usleep(2 * P2OS_CYCLETIME_USEC);
          tcflush(this->psos_fd, TCIFLUSH);
          psos_state = NO_SYNC;
        }
        break;
    }
    usleep(P2OS_CYCLETIME_USEC);
  }
  if (psos_state != READY) {
    if (this->psos_use_tcp) {
      ROS_INFO("Couldn't synchronize with P2OS.\n"
        "  Most likely because the robot is not connected %s %s",
        this->psos_use_tcp ? "to the ethernet-serial bridge device " : "to the serial port",
        this->psos_use_tcp ? this->psos_tcp_host.c_str() : this->psos_serial_port.c_str());
    }
    close(this->psos_fd);
    this->psos_fd = -1;
    return 1;
  }
  cnt = 4;
  cnt += snprintf(name, sizeof(name), "%s", &receivedpacket.packet[cnt]);
  cnt++;
  cnt += snprintf(type, sizeof(type), "%s", &receivedpacket.packet[cnt]);
  cnt++;
  cnt += snprintf(subtype, sizeof(subtype), "%s", &receivedpacket.packet[cnt]);
  cnt++;

  std::string hwID = std::string(name) + ": " + std::string(type) + "/" + std::string(subtype);
  diagnostic_.setHardwareID(hwID);

  command = OPEN;
  packet.Build(&command, 1);
  packet.Send(this->psos_fd);
  usleep(P2OS_CYCLETIME_USEC);
  command = PULSE;
  packet.Build(&command, 1);
  packet.Send(this->psos_fd);
  usleep(P2OS_CYCLETIME_USEC);

  ROS_INFO("Done.\n   Connected to %s, a %s %s", name, type, subtype);

  // now, based on robot type, find the right set of parameters
  for (i = 0; i < PLAYER_NUM_ROBOT_TYPES; i++) {
    if (!strcasecmp(PlayerRobotParams[i].Class.c_str(), type) &&
      !strcasecmp(PlayerRobotParams[i].Subclass.c_str(), subtype))
    {
      param_idx = i;
      break;
    }
  }
  if (i == PLAYER_NUM_ROBOT_TYPES) {
    ROS_WARN("P2OS: Warning: couldn't find parameters for this robot; "
      "using defaults");
    param_idx = 0;
  }

  // first, receive a packet so we know we're connected.
  if (!sippacket) {
    sippacket = new SIP(param_idx);
    sippacket->odom_frame_id = odom_frame_id;
    sippacket->base_link_frame_id = base_link_frame_id;
  }
  /*
    sippacket->x_offset = 0;
    sippacket->y_offset = 0;
    sippacket->angle_offset = 0;

    SendReceive((P2OSPacket*)NULL,false);
  */
  // turn off the sonars at first
  this->ToggleSonarPower(0);
  // if requested, set max accel/decel limits
  P2OSPacket accel_packet;
  unsigned char accel_command[4];
  if (this->motor_max_trans_accel > 0) {
    accel_command[0] = SETA;
    accel_command[1] = ARGINT;
    accel_command[2] = this->motor_max_trans_accel & 0x00FF;
    accel_command[3] = (this->motor_max_trans_accel & 0xFF00) >> 8;
    accel_packet.Build(accel_command, 4);
    this->SendReceive(&accel_packet, false);
  }

  if (this->motor_max_trans_decel < 0) {
    accel_command[0] = SETA;
    accel_command[1] = ARGNINT;
    accel_command[2] = -1 * (this->motor_max_trans_decel) & 0x00FF;
    accel_command[3] = (-1 * (this->motor_max_trans_decel) & 0xFF00) >> 8;
    accel_packet.Build(accel_command, 4);
    this->SendReceive(&accel_packet, false);
  }
  if (this->motor_max_rot_accel > 0) {
    accel_command[0] = SETRA;
    accel_command[1] = ARGINT;
    accel_command[2] = this->motor_max_rot_accel & 0x00FF;
    accel_command[3] = (this->motor_max_rot_accel & 0xFF00) >> 8;
    accel_packet.Build(accel_command, 4);
    this->SendReceive(&accel_packet, false);
  }
  if (this->motor_max_rot_decel < 0) {
    accel_command[0] = SETRA;
    accel_command[1] = ARGNINT;
    accel_command[2] = -1 * (this->motor_max_rot_decel) & 0x00FF;
    accel_command[3] = (-1 * (this->motor_max_rot_decel) & 0xFF00) >> 8;
    accel_packet.Build(accel_command, 4);
    this->SendReceive(&accel_packet, false);
  }


  // if requested, change PID settings
  P2OSPacket pid_packet;
  unsigned char pid_command[4];
  if (this->rot_kp >= 0) {
    pid_command[0] = ROTKP;
    pid_command[1] = ARGINT;
    pid_command[2] = this->rot_kp & 0x00FF;
    pid_command[3] = (this->rot_kp & 0xFF00) >> 8;
    pid_packet.Build(pid_command, 4);
    this->SendReceive(&pid_packet);
  }
  if (this->rot_kv >= 0) {
    pid_command[0] = ROTKV;
    pid_command[1] = ARGINT;
    pid_command[2] = this->rot_kv & 0x00FF;
    pid_command[3] = (this->rot_kv & 0xFF00) >> 8;
    pid_packet.Build(pid_command, 4);
    this->SendReceive(&pid_packet);
  }
  if (this->rot_ki >= 0) {
    pid_command[0] = ROTKI;
    pid_command[1] = ARGINT;
    pid_command[2] = this->rot_ki & 0x00FF;
    pid_command[3] = (this->rot_ki & 0xFF00) >> 8;
    pid_packet.Build(pid_command, 4);
    this->SendReceive(&pid_packet);
  }
  if (this->trans_kp >= 0) {
    pid_command[0] = TRANSKP;
    pid_command[1] = ARGINT;
    pid_command[2] = this->trans_kp & 0x00FF;
    pid_command[3] = (this->trans_kp & 0xFF00) >> 8;
    pid_packet.Build(pid_command, 4);
    this->SendReceive(&pid_packet);
  }
  if (this->trans_kv >= 0) {
    pid_command[0] = TRANSKV;
    pid_command[1] = ARGINT;
    pid_command[2] = this->trans_kv & 0x00FF;
    pid_command[3] = (this->trans_kv & 0xFF00) >> 8;
    pid_packet.Build(pid_command, 4);
    this->SendReceive(&pid_packet);
  }
  if (this->trans_ki >= 0) {
    pid_command[0] = TRANSKI;
    pid_command[1] = ARGINT;
    pid_command[2] = this->trans_ki & 0x00FF;
    pid_command[3] = (this->trans_ki & 0xFF00) >> 8;
    pid_packet.Build(pid_command, 4);
    this->SendReceive(&pid_packet);
  }


  // if requested, change bumper-stall behavior
  // 0 = don't stall
  // 1 = stall on front bumper contact
  // 2 = stall on rear bumper contact
  // 3 = stall on either bumper contact
  if (this->bumpstall >= 0) {
    if (this->bumpstall > 3) {
      ROS_INFO("ignoring bumpstall value %d; should be 0, 1, 2, or 3",
        this->bumpstall);
    } else {
      ROS_INFO("setting bumpstall to %d", this->bumpstall);
      P2OSPacket bumpstall_packet;
      unsigned char bumpstall_command[4];
      bumpstall_command[0] = BUMP_STALL;
      bumpstall_command[1] = ARGINT;
      bumpstall_command[2] = (unsigned char)this->bumpstall;
      bumpstall_command[3] = 0;
      bumpstall_packet.Build(bumpstall_command, 4);
      this->SendReceive(&bumpstall_packet, false);
    }
  }

  // Turn on the sonar
  if (use_sonar_) {
    this->ToggleSonarPower(1);
    ROS_DEBUG("Sonar array powered on.");
  }
  ptz_.setup();

  return 0;
}

int P2OSNode::Shutdown()
{
  unsigned char command[20], buffer[20];
  P2OSPacket packet;

  if (ptz_.isOn()) {
    ptz_.shutdown();
  }

  memset(buffer, 0, 20);

  if (this->psos_fd == -1) {
    return -1;
  }

  command[0] = STOP;
  packet.Build(command, 1);
  packet.Send(this->psos_fd);
  usleep(P2OS_CYCLETIME_USEC);

  command[0] = CLOSE;
  packet.Build(command, 1);
  packet.Send(this->psos_fd);
  usleep(P2OS_CYCLETIME_USEC);

  close(this->psos_fd);
  this->psos_fd = -1;
  ROS_INFO("P2OS has been shutdown");
  delete this->sippacket;
  this->sippacket = NULL;

  return 0;
}


void
P2OSNode::StandardSIPPutData(ros::Time ts)
{
  p2os_data.position.header.stamp = ts;
  pose_pub_.publish(p2os_data.position);
  p2os_data.odom_trans.header.stamp = ts;
  odom_broadcaster.sendTransform(p2os_data.odom_trans);

  p2os_data.batt.header.stamp = ts;
  batt_pub_.publish(p2os_data.batt);
  mstate_pub_.publish(p2os_data.motors);

  // put sonar data
  p2os_data.sonar.header.stamp = ts;
  sonar_pub_.publish(p2os_data.sonar);

  // put aio data
  aio_pub_.publish(p2os_data.aio);
  // put dio data
  dio_pub_.publish(p2os_data.dio);

  // put gripper and lift data
  grip_state_pub_.publish(p2os_data.gripper);
  ptz_state_pub_.publish(ptz_.getCurrentState());

  // put bumper data
  // put compass data
}

/* send the packet, then receive and parse an SIP */
int P2OSNode::SendReceive(P2OSPacket * pkt, bool publish_data)
{
  P2OSPacket packet;

  if ((this->psos_fd >= 0) && this->sippacket) {
    if (pkt) {
      pkt->Send(this->psos_fd);
    }

    /* receive a packet */
    pthread_testcancel();
    if (packet.Receive(this->psos_fd)) {
      ROS_ERROR("RunPsosThread(): Receive errored");
      pthread_exit(NULL);
    }

    const bool packet_check =
      packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
      (packet.packet[3] == 0x30 || packet.packet[3] == 0x31 ||
      packet.packet[3] == 0x32 || packet.packet[3] == 0x33 ||
      packet.packet[3] == 0x34);
    const bool ser_aux =
      (packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && packet.packet[3] == SERAUX);
    if (packet_check) {
      /* It is a server packet, so process it */
      this->sippacket->ParseStandard(&packet.packet[3]);
      this->sippacket->FillStandard(&(this->p2os_data));

      if (publish_data) {
        this->StandardSIPPutData(packet.timestamp);
      }
    } else if (ser_aux) {
      // This is an AUX serial packet
      if (ptz_.isOn()) {
        int len = packet.packet[2] - 3;
        if (ptz_.cb_.gotPacket()) {
          ROS_ERROR("PTZ got a message, but alread has the complete packet.");
        } else {
          for (int i = 4; i < 4 + len; ++i) {
            ptz_.cb_.putOnBuf(packet.packet[i]);
          }
        }
      }
    } else {
      ROS_ERROR("Received other packet!");
      packet.PrintHex();
    }
  }

  return 0;
}

void P2OSNode::updateDiagnostics()
{
  diagnostic_.update();
}

void P2OSNode::check_voltage(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  double voltage = sippacket->battery / 10.0;
  if (voltage < 11.0) {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "battery voltage critically low");
  } else if (voltage < 11.75) {
    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "battery voltage getting low");
  } else {stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "battery voltage OK");}

  stat.add("voltage", voltage);
}

void P2OSNode::check_stall(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  if (sippacket->lwstall || sippacket->rwstall) {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
      "wheel stalled");
  } else {stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "no wheel stall");}

  stat.add("left wheel stall", sippacket->lwstall);
  stat.add("right wheel stall", sippacket->rwstall);
}

void P2OSNode::ResetRawPositions()
{
  P2OSPacket pkt;
  unsigned char p2oscommand[4];

  if (this->sippacket) {
    this->sippacket->rawxpos = 0;
    this->sippacket->rawypos = 0;
    this->sippacket->xpos = 0;
    this->sippacket->ypos = 0;
    p2oscommand[0] = SETO;
    p2oscommand[1] = ARGINT;
    pkt.Build(p2oscommand, 2);
    this->SendReceive(&pkt, false);
    ROS_INFO("resetting raw positions");
  }
}

/* toggle sonars on/off, according to val */
void P2OSNode::ToggleSonarPower(unsigned char val)
{
  unsigned char command[4];
  P2OSPacket packet;

  command[0] = SONAR;
  command[1] = ARGINT;
  command[2] = val;
  command[3] = 0;
  packet.Build(command, 4);
  SendReceive(&packet, false);
}

void P2OSNode::ToggleMotorPower(unsigned char val)
{
  unsigned char command[4];
  P2OSPacket packet;
  ROS_INFO("motor state: %d\n", p2os_data.motors.state);
  p2os_data.motors.state = static_cast<int>(val);
  command[0] = ENABLE;
  command[1] = ARGINT;
  command[2] = val;
  command[3] = 0;
  packet.Build(command, 4);
  SendReceive(&packet, false);
}

//  Actarray stuff

// Ticks to degrees from the ARIA software
inline double P2OSNode::TicksToDegrees(int joint, unsigned char ticks)
{
  if ((joint < 0) || (joint >= sippacket->armNumJoints)) {
    return 0;
  }

  double result;
  int pos = ticks - sippacket->armJoints[joint].centre;
  result = 90.0 / static_cast<double>(sippacket->armJoints[joint].ticksPer90);
  result = result * pos;
  if ((joint >= 0) && (joint <= 2)) {
    result = -result;
  }

  return result;
}

// Degrees to ticks from the ARIA software
inline unsigned char P2OSNode::DegreesToTicks(int joint, double degrees)
{
  double val;

  if ((joint < 0) || (joint >= sippacket->armNumJoints)) {
    return 0;
  }

  val = static_cast<double>(sippacket->armJoints[joint].ticksPer90) * degrees / 90.0;
  val = round(val);
  if ((joint >= 0) && (joint <= 2)) {
    val = -val;
  }
  val += sippacket->armJoints[joint].centre;

  if (val < sippacket->armJoints[joint].min) {
    return sippacket->armJoints[joint].min;
  } else if (val > sippacket->armJoints[joint].max) {return sippacket->armJoints[joint].max;} else {
    return static_cast<int>(round(val));
  }
}

inline double P2OSNode::TicksToRadians(int joint, unsigned char ticks)
{
  double result = DTOR(TicksToDegrees(joint, ticks));
  return result;
}

inline unsigned char P2OSNode::RadiansToTicks(int joint, double rads)
{
  unsigned char result = static_cast<unsigned char>(DegreesToTicks(joint, RTOD(rads)));
  return result;
}

inline double P2OSNode::RadsPerSectoSecsPerTick(int joint, double speed)
{
  double degs = RTOD(speed);
  double ticksPerDeg = static_cast<double>(sippacket->armJoints[joint].ticksPer90) / 90.0;
  double ticksPerSec = degs * ticksPerDeg;
  double secsPerTick = 1000.0f / ticksPerSec;

  if (secsPerTick > 127) {
    return 127;
  } else if (secsPerTick < 1) {
    return 1;
  }
  return secsPerTick;
}

inline double P2OSNode::SecsPerTicktoRadsPerSec(int joint, double msecs)
{
  double ticksPerSec = 1.0 / (static_cast<double>(msecs) / 1000.0);
  double ticksPerDeg = static_cast<double>(sippacket->armJoints[joint].ticksPer90) / 90.0;
  double degs = ticksPerSec / ticksPerDeg;
  double rads = DTOR(degs);

  return rads;
}

void P2OSNode::SendPulse(void)
{
  unsigned char command;
  P2OSPacket packet;

  command = PULSE;
  packet.Build(&command, 1);
  SendReceive(&packet);
}