simple_keyop.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2013, Yujin Robot.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Yujin Robot nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*****************************************************************************
 ** Includes
 *****************************************************************************/

#include <string>
#include <csignal>
#include <termios.h> // for keyboard input
#include <ecl/console.hpp>
#include <ecl/exceptions.hpp>
#include <ecl/geometry/legacy_pose2d.hpp>
#include <ecl/linear_algebra.hpp>
#include <ecl/sigslots.hpp>
#include <ecl/time.hpp>
#include <ecl/threads.hpp>

#include "kobuki_driver/kobuki.hpp"

/*****************************************************************************
** Classes
*****************************************************************************/

class KobukiManager
{
public:
  /*********************
   ** C&D
   **********************/
  KobukiManager();
  ~KobukiManager();
  bool init();

  /*********************
   ** Runtime
   **********************/
  void spin();

  /*********************
   ** Callbacks
   **********************/
  void processStreamData();

  /*********************
   ** Accessor
   **********************/
  ecl::LegacyPose2D<double> getPose();
  bool isShutdown() const { return quit_requested || kobuki.isShutdown(); }

private:
  double vx, wz;
  ecl::LegacyPose2D<double> pose;
  kobuki::Kobuki kobuki;
  ecl::Slot<const std::string&> slot_debug_error, slot_debug_warning;
  ecl::Slot<> slot_stream_data;

  double linear_vel_step, linear_vel_max;
  double angular_vel_step, angular_vel_max;
  std::string name;

  /*********************
   ** Commands
   **********************/
  void incrementLinearVelocity();
  void decrementLinearVelocity();
  void incrementAngularVelocity();
  void decrementAngularVelocity();
  void resetVelocity();

  /*********************
   ** Logging
   **********************/
  void relayWarnings(const std::string& message);
  void relayErrors(const std::string& message);

  /*********************
   ** Keylogging
   **********************/

  void keyboardInputLoop();
  void processKeyboardInput(char c);
  void restoreTerminal();
  bool quit_requested;
  int key_file_descriptor;
  struct termios original_terminal_state;
  ecl::Thread thread;
};

/*****************************************************************************
 ** Implementation
 *****************************************************************************/

KobukiManager::KobukiManager() :
                         linear_vel_step(0.05),
                         linear_vel_max(1.0),
                         angular_vel_step(0.33),
                         angular_vel_max(6.6),
                         quit_requested(false),
                         key_file_descriptor(0),
                         vx(0.0), wz(0.0),
                         slot_debug_error(&KobukiManager::relayErrors, *this),
                         slot_debug_warning(&KobukiManager::relayWarnings, *this),
                         slot_stream_data(&KobukiManager::processStreamData, *this)
{
  tcgetattr(key_file_descriptor, &original_terminal_state); // get terminal properties
}

KobukiManager::~KobukiManager()
{
  kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
  kobuki.disable();
  tcsetattr(key_file_descriptor, TCSANOW, &original_terminal_state);
}

bool KobukiManager::init()
{
  /*********************
   ** Parameters
   **********************/
  std::cout << "Parameters" << std::endl;
  std::cout << "----------" << std::endl;
  std::cout << " - linear_vel_max   [" << linear_vel_max << "]" << std::endl;
  std::cout << " - linear_vel_step  [" << linear_vel_step << "]" << std::endl;
  std::cout << " - angular_vel_max  [" << angular_vel_max << "]" << std::endl;
  std::cout << " - angular_vel_step [" << angular_vel_step << "]" << std::endl;
  std::cout << std::endl;

  /*********************
   ** Velocities
   **********************/
  vx = 0.0;
  wz = 0.0;

  /*********************
   ** Kobuki
   **********************/
  kobuki::Parameters parameters;
  parameters.sigslots_namespace = "/kobuki";
  parameters.device_port = "/dev/kobuki";
  parameters.enable_acceleration_limiter = true;

  kobuki.init(parameters);
  kobuki.enable();
  slot_debug_warning.connect("/kobuki/ros_warn");
  slot_debug_error.connect("/kobuki/ros_error");
  slot_stream_data.connect("/kobuki/stream_data");

  /*********************
   ** Wait for connection
   **********************/
  thread.start(&KobukiManager::keyboardInputLoop, *this);
  return true;
}

/*****************************************************************************
 ** Implementation [Spin]
 *****************************************************************************/

void KobukiManager::spin()
{
/*
  {
    // just in case we got here not via a keyboard quit request
    quit_requested = true;
    thread.cancel();
  }
*/
  ecl::Sleep sleep(ecl::Duration(0.1));
  while (!quit_requested){
    sleep();
  }
  thread.join();
}

/*****************************************************************************
 ** Implementation [Logging]
 *****************************************************************************/

void KobukiManager::relayWarnings(const std::string& message) {
  std::cout << ecl::yellow << "[WARNING] " << message << ecl::reset << std::endl;
}

void KobukiManager::relayErrors(const std::string& message) {
  std::cout << ecl::red << "[ERROR] " << message << ecl::reset << std::endl;
}

/*****************************************************************************
 ** Implementation [Keyboard]
 *****************************************************************************/

void KobukiManager::keyboardInputLoop()
{
  struct termios raw;
  memcpy(&raw, &original_terminal_state, sizeof(struct termios));

  raw.c_lflag &= ~(ICANON | ECHO);
  // Setting a new line, then end of file
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(key_file_descriptor, TCSANOW, &raw);

  std::cout << "Reading from keyboard" << std::endl;
  std::cout << "---------------------" << std::endl;
  std::cout << "Forward/back arrows : linear velocity incr/decr." << std::endl;
  std::cout << "Right/left arrows : angular velocity incr/decr." << std::endl;
  std::cout << "Spacebar : reset linear/angular velocities." << std::endl;
  std::cout << "q : quit." << std::endl;
  std::cout << std::endl;
  char c;
  while (!quit_requested)
  {
    if (read(key_file_descriptor, &c, 1) < 0)
    {
      perror("read char failed():");
      exit(-1);
    }
    processKeyboardInput(c);
  }
}

void KobukiManager::processKeyboardInput(char c)
{
  /*
   * Arrow keys are a bit special, they are escape characters - meaning they
   * trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We
   * ignore the esc-[ and just parse the last one. So long as we avoid using
   * the last one for its actual purpose (e.g. left arrow corresponds to
   * esc-[-D) we can keep the parsing simple.
   */
  switch (c)
  {
    case 68://kobuki_msgs::KeyboardInput::KeyCode_Left:
    {
      incrementAngularVelocity();
      break;
    }
    case 67://kobuki_msgs::KeyboardInput::KeyCode_Right:
    {
      decrementAngularVelocity();
      break;
    }
    case 65://kobuki_msgs::KeyboardInput::KeyCode_Up:
    {
      incrementLinearVelocity();
      break;
    }
    case 66://kobuki_msgs::KeyboardInput::KeyCode_Down:
    {
      decrementLinearVelocity();
      break;
    }
    case 32://kobuki_msgs::KeyboardInput::KeyCode_Space:
    {
      resetVelocity();
      break;
    }
    case 'q':
    {
      quit_requested = true;
      break;
    }
    default:
    {
      break;
    }
  }
}

/*****************************************************************************
 ** Implementation [Commands]
 *****************************************************************************/

void KobukiManager::incrementLinearVelocity()
{
  if (vx <= linear_vel_max)
  {
    vx += linear_vel_step;
  }
//  ROS_INFO_STREAM("KeyOp: linear  velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}

void KobukiManager::decrementLinearVelocity()
{
  if (vx >= -linear_vel_max)
  {
    vx -= linear_vel_step;
  }
//  ROS_INFO_STREAM("KeyOp: linear  velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}

void KobukiManager::incrementAngularVelocity()
{
  if (wz <= angular_vel_max)
  {
    wz += angular_vel_step;
  }
//  ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}

void KobukiManager::decrementAngularVelocity()
{
  if (wz >= -angular_vel_max)
  {
    wz -= angular_vel_step;
  }
//    ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
}

void KobukiManager::resetVelocity()
{
  vx = 0.0;
  wz = 0.0;
//    ROS_INFO_STREAM("KeyOp: reset linear/angular velocities.");
}

void KobukiManager::processStreamData() {
  ecl::LegacyPose2D<double> pose_update;
  ecl::linear_algebra::Vector3d pose_update_rates;
  kobuki.updateOdometry(pose_update, pose_update_rates);
  pose *= pose_update;
//  dx += pose_update.x();
//  dth += pose_update.heading();
  //std::cout << dx << ", " << dth << std::endl;
  //std::cout << kobuki.getHeading() << ", " << pose.heading() << std::endl;
  //std::cout << "[" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;

  kobuki.setBaseControl(vx, wz);
}

ecl::LegacyPose2D<double> KobukiManager::getPose() {
  return pose;
}

/*****************************************************************************
** Signal Handler
*****************************************************************************/

bool shutdown_req = false;
void signalHandler(int signum) {
  shutdown_req = true;
}

/*****************************************************************************
** Main
*****************************************************************************/

int main(int argc, char** argv)
{
  signal(SIGINT, signalHandler);

  std::cout << ecl::bold << "\nSimple Keyop : Utility for driving kobuki by keyboard.\n" << ecl::reset << std::endl;
  KobukiManager kobuki_manager;
  kobuki_manager.init();

  ecl::Sleep sleep(1);
  ecl::LegacyPose2D<double> pose;
  try {
    while (!shutdown_req && !kobuki_manager.isShutdown()){
      sleep();
      pose = kobuki_manager.getPose();
      std::cout << ecl::green;
      std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
      std::cout << ecl::reset;
    }
  } catch ( ecl::StandardException &e ) {
    std::cout << e.what();
  }
  return 0;
}