ROS Package

.gitignore

@@ -0,0 +1,3 @@
+.swp
+.save
+.bak

CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(teleop_twist_keyboard_cpp)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  geometry_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS src
+  CATKIN_DEPENDS roscpp geometry_msgs 
+  DEPENDS system_lib map
+)
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_executable(teleop_twist_keyboard src/teleop_twist_keyboard.cpp)
+target_link_libraries(teleop_twist_keyboard ${catkin_LIBRARIES})
+
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")

README.md

@@ -1,2 +1,75 @@
 # teleop_twist_keyboard_cpp
 C++ Implementation of the Generic Keyboard Teleop for ROS: https://github.com/ros-teleop/teleop_twist_keyboard
+
+## Features
+
+This particular implementation does away with keeping the history of previous speed settings, and heavily cuts down on the amount of printing that is done to the terminal via the use of carriage returns (\r).
+
+Furthermore, the last command that was sent is reflected, and invalid commands are identified as such.
+
+
+
+## Installing the Package
+
+As per standard ROS practice, make a workspace, go to the workspace's src directory, and clone this repository, then run catkin_make in the root of the workspace, and source the resulting setup.bash!
+
+```bash
+$ git clone https://github.com/methylDragon/teleop_twist_keyboard_cpp.git
+$ cd ..
+$ catkin_make
+
+$ source devel/setup.bash
+```
+
+
+
+## Running the Node
+
+```bash
+# In one terminal, run
+$ roscore
+
+# In another terminal, run
+$ rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard
+
+# If you want to see the outputs, check the /cmd_vel topic
+$ rostopic echo /cmd_vel
+```
+
+
+
+## Usage
+
+Same as the original
+
+```
+Reading from the keyboard  and Publishing to Twist!
+---------------------------
+Moving around:
+   u    i    o
+   j    k    l
+   m    ,    .
+
+For Holonomic mode (strafing), hold down the shift key:
+---------------------------
+   U    I    O
+   J    K    L
+   M    <    >
+
+t : up (+z)
+b : down (-z)
+
+anything else : stop
+
+q/z : increase/decrease max speeds by 10%
+w/x : increase/decrease only linear speed by 10%
+e/c : increase/decrease only angular speed by 10%
+
+CTRL-C to quit
+```
+
+
+
+------
+
+[![Yeah! Buy the DRAGON a COFFEE!](./_assets/COFFEE%20BUTTON%20%E3%83%BE(%C2%B0%E2%88%87%C2%B0%5E).png)](https://www.buymeacoffee.com/methylDragon)

package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>teleop_twist_keyboard_cpp</name>
+  <version>0.0.0</version>
+  <description>Generic keyboard teleop for twist robots (in C++)! Based off of the teleop_twist_keyboard Python ROS node.</description>
+
+  <author email="[email protected]">methylDragon</author>
+  <maintainer email="[email protected]">methylDragon</maintainer>
+  <url type="website">http://github.com/methylDragon</url>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+</package>

src/teleop_twist_keyboard.cpp

@@ -0,0 +1,186 @@
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+
+#include <stdio.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <map>
+
+// Map for movement keys
+std::map<char, std::vector<float>> moveBindings
+{
+  {'i', {1, 0, 0, 0}},
+  {'o', {1, 0, 0, -1}},
+  {'j', {0, 0, 0, 1}},
+  {'l', {0, 0, 0, -1}},
+  {'u', {-1, 0, 0, 0}},
+  {',', {-1, 0, 0, 1}},
+  {'.', {-1, 0, 0, 1}},
+  {'m', {-1, 0, 0, -1}},
+  {'O', {1, -1, 0, 0}},
+  {'I', {1, 0, 0, 0}},
+  {'J', {0, 1, 0, 0}},
+  {'L', {0, -1, 0, 0}},
+  {'U', {1, 1, 0, 0}},
+  {'<', {-1, 0, 0, 0}},
+  {'>', {-1, -1, 0, 0}},
+  {'M', {-1, 1, 0, 0}},
+  {'t', {0, 0, 1, 0}},
+  {'b', {0, 0, -1, 0}},
+  {'k', {0, 0, 0, 0}},
+  {'K', {0, 0, 0, 0}}
+};
+
+
+// Map for speed keys
+std::map<char, std::vector<float>> speedBindings
+{
+  {'q', {1.1, 1.1}},
+  {'z', {0.9, 0.9}},
+  {'w', {1.1, 1}},
+  {'x', {0.9, 1}},
+  {'e', {1, 1.1}},
+  {'c', {1, 0.9}}
+};
+
+// Reminder message
+const char* msg = R"(
+
+Reading from the keyboard and Publishing to Twist!
+---------------------------
+Moving around:
+   u    i    o
+   j    k    l
+   m    ,    .
+
+For Holonomic mode (strafing), hold down the shift key:
+---------------------------
+   U    I    O
+   J    K    L
+   M    <    >
+
+t : up (+z)
+b : down (-z)
+
+anything else : stop
+
+q/z : increase/decrease max speeds by 10%
+w/x : increase/decrease only linear speed by 10%
+e/c : increase/decrease only angular speed by 10%
+
+CTRL-C to quit
+
+)";
+
+// Init variables
+float speed(0.5); // Linear velocity (m/s)
+float turn(1.0); // Angular velocity (rad/s)
+float x(0), y(0), z(0), th(0); // Forward/backward/neutral direction vars
+char key(' ');
+
+// For non-blocking keyboard inputs
+int getch(void)
+{
+  int ch;
+  struct termios oldt;
+  struct termios newt;
+
+  // Store old settings, and copy to new settings
+  tcgetattr(STDIN_FILENO, &oldt);
+  newt = oldt;
+
+  // Make required changes and apply the settings
+  newt.c_lflag &= ~(ICANON | ECHO);
+  newt.c_iflag |= IGNBRK;
+  newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
+  newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
+  newt.c_cc[VMIN] = 1;
+  newt.c_cc[VTIME] = 0;
+  tcsetattr(fileno(stdin), TCSANOW, &newt);
+
+  // Get the current character
+  ch = getchar();
+
+  // Reapply old settings
+  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+
+  return ch;
+}
+
+int main(int argc, char** argv)
+{
+  // Init ROS node
+  ros::init(argc, argv, "teleop_twist_keyboard");
+  ros::NodeHandle nh;
+
+  // Init cmd_vel publisher
+  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+
+  // Create Twist message
+  geometry_msgs::Twist twist;
+
+  printf("%s", msg);
+  printf("\rCurrent: speed %f\tturn %f | Last command: %c\r", speed, turn, key);
+
+  while(true){
+
+    // Get the pressed key
+    key = getch();
+
+    // If the key corresponds to a key in moveBindings
+    if (moveBindings.count(key) == 1)
+    {
+      // Grab the direction data
+      x = moveBindings[key][0];
+      y = moveBindings[key][1];
+      z = moveBindings[key][2];
+      th = moveBindings[key][3];
+
+      printf("\rCurrent: speed %f\tturn %f | Last command: %c   ", speed, turn, key);
+    }
+
+    // Otherwise if it corresponds to a key in speedBindings
+    else if (speedBindings.count(key) == 1)
+    {
+      // Grab the speed data
+      speed = speed * speedBindings[key][0];
+      turn = turn * speedBindings[key][1];
+
+      printf("\rCurrent: speed %f\tturn %f | Last command: %c   ", speed, turn, key);
+    }
+
+    // Otherwise, set the robot to stop
+    else
+    {
+      x = 0;
+      y = 0;
+      z = 0;
+      th = 0;
+
+      // If ctrl-C (^C) was pressed, terminate the program
+      if (key == '\x03')
+      {
+        printf("\n\n                 .     .\n              .  |\\-^-/|  .    \n             /| } O.=.O { |\\\n\n                 CH3EERS\n\n");
+        break;
+      }
+
+      printf("\rCurrent: speed %f\tturn %f | Invalid command! %c", speed, turn, key);
+    }
+
+    // Update the Twist message
+    twist.linear.x = x * speed;
+    twist.linear.y = y * speed;
+    twist.linear.z = z * speed;
+
+    twist.angular.x = 0;
+    twist.angular.y = 0;
+    twist.angular.z = th * turn;
+
+    // Publish it and resolve any remaining callbacks
+    pub.publish(twist);
+    ros::spinOnce();
+  }
+
+  return 0;
+}