keyboard_interface.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  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 the Willow Garage 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.
 *********************************************************************/


#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64.h>

#include <unistd.h>
#include <termios.h>
#include <cstdio>

namespace offsetter {

  enum ShiftMode { ROTATION=0, TRANSLATION=1, BASELINE=2 };

  void printUsage()
  {
    printf("\n  Stereo camera offsetter usage:\n\n");
                printf("  t -> switch to translational mode.\n");
                printf("  r -> switch to rotational mode.\n");
                printf("  b -> switch to baseline mode.\n");
  }

  void printPrompt(enum ShiftMode mode)
  {
    printf("\n");
    switch (mode)
      {
      case TRANSLATION:
                                {
                                        printf("      translation mode: press x/X y/Y z/Z to translate >> ");
                                        break;
                                }
      case ROTATION:
                                {
                                        printf("          rotational mode: press x/X y/Y z/Z to rotate >> ");
                                        break;
                                }
      case BASELINE:
                                {
                                        printf("baseline mode: press x/X to decrease/increase baseline >> ");
                                        break;
                                }
      default:
                                printUsage();
      }
  }

}

using namespace offsetter;
using namespace std;
using namespace ros;

int main(int argc, char** argv)
{
  // Setup terminal settings for getchar
  const int fd = fileno(stdin);
  termios prev_flags ;
  tcgetattr(fd, &prev_flags) ;
  termios flags ;
  tcgetattr(fd,&flags);
  flags.c_lflag &= ~ICANON;  // set raw (unset canonical modes)
  flags.c_cc[VMIN]  = 0;     // i.e. min 1 char for blocking, 0 chars for non-blocking
  flags.c_cc[VTIME] = 0;     // block if waiting for char
  tcsetattr(fd,TCSANOW,&flags);

  ros::init(argc, argv, "stereo_offsetter_keyboard_interface");

  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("virtual_twist", 10);

  ros::Publisher baseline_pub = nh.advertise<std_msgs::Float64>("baseline_factor", 10);

  static const double trans = .001;
  static const double rot   = .1 * 3.14 / 180.0;
  static const double base  = .005;

  ShiftMode cur_shift_mode = offsetter::TRANSLATION;

  printUsage();
  printPrompt(cur_shift_mode);

  while (nh.ok())
    {
      char c = getchar();

      geometry_msgs::Twist twist;
      geometry_msgs::Vector3 base_vec;

      geometry_msgs::Vector3* vec = NULL;
      double shift_val;
      switch (cur_shift_mode)
                                {
                                case ROTATION:
                                        vec = &twist.angular;
                                        shift_val = rot;
                                        break;
                                case TRANSLATION:
                                        vec = &twist.linear;
                                        shift_val = trans;
                                        break;
                                case BASELINE:
                                        vec = &base_vec;
                                        shift_val = base;
                                        break;
                                default:
                                        shift_val = 0.0;
                                        ROS_FATAL("unknown shift type [%u]", cur_shift_mode);
                                        break;
                                }

      bool should_publish = false;

      switch (c)
                                {
                                case 'x': vec->x = -shift_val; printPrompt(cur_shift_mode); should_publish = true; break;
                                case 'X': vec->x =  shift_val; printPrompt(cur_shift_mode); should_publish = true; break;
                                case 'y': vec->y = -shift_val; printPrompt(cur_shift_mode); should_publish = true; break;
                                case 'Y': vec->y =  shift_val; printPrompt(cur_shift_mode); should_publish = true; break;
                                case 'z': vec->z = -shift_val; printPrompt(cur_shift_mode); should_publish = true; break;
                                case 'Z': vec->z =  shift_val; printPrompt(cur_shift_mode); should_publish = true; break;
                                case 't':
                                case 'T':
                                        cur_shift_mode = TRANSLATION;
                                        printPrompt(cur_shift_mode);
                                        break;
                                case 'r':
                                case 'R':
                                        cur_shift_mode = ROTATION;
                                        printPrompt(cur_shift_mode);
                                        break;
                                case 'b':
                                case 'B':
                                        cur_shift_mode = BASELINE;
                                        printPrompt(cur_shift_mode);
                                        break;
                                case EOF:
                                        usleep(100);
                                        break;
                                default:
                                        usleep(100);
                                        printUsage();
                                        printPrompt(cur_shift_mode);
                                        break;
                                }
    
      if (should_publish)
                                {
                                        //       ROS_INFO("Publishing Twist: Translation(%.3f, %.3f, %.3f)    Rotation(%.3f, %.3f, %.3f)   Baseline(%f)",
                                        //                twist.linear.x,  twist.linear.y,  twist.linear.z,
                                        //                twist.angular.x, twist.angular.y, twist.angular.z, base_vec.x);
                                        pub.publish(twist);

                                        std_msgs::Float64 baseline;
                                        baseline.data = vec->x;
                                        baseline_pub.publish(baseline);

                                }
    }

  tcsetattr(fd,TCSANOW, &prev_flags) ;         // Undo any terminal changes that we made

  return 0;
}