Creating a ROS Package and C++ Node to Drive the iRobot Create

Today I'm going to go through the process I went through to create a simple node to drive the iRobot Create. I discussed last post how to get the driver for iCreate working in ROS Hydro. I want to make a package which contains a program that will allow me to drive the create using the w, a, s, and d keys on my keyboard. To get started, I need to navigate to my ROS workspace. Don't forget to source your ROS and ROS workspace settings.
source /opt/ros/hydro/setup.bash
source ~/catkin_ws/devel/setup.bash     # my workspace location

Now, in the workspace directory:
cd ~/catkin_ws/src  # navigate to my workspace src directory
catkin_create_pkg robot_mover geometry_msgs std_msgs genmsg      roscpp rospy
cd .. # return to workspace directory to make
catkin_make

Now, to create the program:
roscd robot_mover/src
gedit keyboard_mover.cpp

Inside of keyboard_mover file I used the following code, modified from here:

#include <iostream>
#include <stdio.h>
#include <ncurses.h>

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

class RobotDriver
{
private:
  //! The node handle we'll be using
  ros::NodeHandle n_;
  //! We will be publishing to the "/base_controller/command" topic to issue commands
  ros::Publisher cmd_vel_pub_;

public:
  //! ROS node initialization
  RobotDriver(ros::NodeHandle &n)
  {
    n_ = n;
    //set up the publisher for the cmd_vel topic
    cmd_vel_pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
  }

  //! Loop forever while sending drive commands based on keyboard input
  bool driveKeyboard()
  {
    std::cout << "Type a command and then press enter.  "
      "Use 'w','a','s', and 'd' to navigate and 'q' to exit.\n";

    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;

    char cmd;
    initscr(); //get terminal environment variables
    cbreak();  //line buffering disabled; pass on everything
    timeout(1000);  //getch blocks for 1 second

    while(n_.ok()){

      cmd = getch();

      if(cmd !='w' && cmd !='a' && cmd !='s' && cmd !='d' && cmd !='q' && cmd != 'e' && cmd != -1)
      {
        std::cout << "unknown command...\n\r";
        continue;
      }

      base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0.0;  
      //move forward
      if(cmd =='w'){
        base_cmd.linear.x = 0.25;
      }
      //move backwards
      if(cmd =='s'){
        base_cmd.linear.x = -0.25;
      }
      //turn left (yaw) and drive forward at the same time
      else if(cmd =='a'){
        base_cmd.angular.z = 0.75;
        base_cmd.linear.x = 0.0;
      }
      //turn right (yaw) and drive forward at the same time
      else if(cmd =='d'){
        base_cmd.angular.z = -0.75;
        base_cmd.linear.x = 0.0;
      }
      else if(cmd =='d'){
        base_cmd.angular.z = 0.0;
        base_cmd.linear.x = 0.0;
      }
      else if(cmd == -1){
        base_cmd.angular.z = 0.0;
        base_cmd.linear.x = 0.0;
      }        
      //quit
      else if(cmd =='q'){
        base_cmd.angular.z = 0.0;
        base_cmd.linear.x = 0.0;
        break;
      }

      //publish the assembled command
      cmd_vel_pub_.publish(base_cmd);

      std::cout << "\n\r";
    }
    nocbreak(); //return terminal to "cooked" mode
    return true;
  }

};

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "robot_driver");
  ros::NodeHandle n;

  RobotDriver driver(n);
  driver.driveKeyboard();
}

Of course, I wanted this code to work without having to press enter in the terminal after each command, and I also wanted the robot to stop after not receiving a new command. I used the ncurses++ library to get characters from the terminal immediately using the getch() function. To use an external library, the CMakeList file and the package file needed to be modified.

At the end of my CMakeList.txt file I added:
## Build keyboard_mover
find_package( PkgConfig REQUIRED )
pkg_check_modules( ncurses++ REQUIRED ncurses++ )
add_executable(keyboard_mover src/keyboard_mover.cpp)
target_link_libraries(keyboard_mover ${catkin_LIBRARIES})
target_link_libraries(keyboard_mover ${ncurses++_LIBRARIES})
add_dependencies(keyboard_mover ${catkin_EXPORTED_TARGETS})

And in my package.xml I added:
<build_depend>ncurses++</build_depend>

And now I am able to drive the Create around after compiling with catkin_make. Note: you may need to install the ncurses library if you don't already have it.
sudo apt-get install ncurses-*
To run this you must run the following commands in different terminal windows in the following order:
roscore
rosrun irobot_create_2_1 # you may need to chmod 666 your tty/USB0 for this to work
rosrun robot_mover keyboard_mover


Update: I upgraded to ROS Indigo, and to get catkin_make to work again, I had to modify the CMakeLists.txt as follows:
## Build keyboard_mover
find_package( PkgConfig REQUIRED )
pkg_check_modules( ncurses++ REQUIRED ncurses++ )
add_executable(keyboard_mover src/keyboard_mover.cpp)
target_link_libraries(keyboard_mover ${catkin_LIBRARIES} ncurses)
target_link_libraries(keyboard_mover ${ncurses++_LIBRARIES})
add_dependencies(keyboard_mover ${catkin_EXPORTED_TARGETS} keyboard_mover)

iRobot Create ROS Hydro Drivers

I recently started messing around with ROS. I have a few common robotics development platforms laying around my lab, including an iRobot Create, which is essentially the Roomba without the vacuum parts. A lot of ROS projects are developed using the Create, so there's plenty of resources to pull from online.

Brown University has provided drivers for the Create, but the most recent ROS distro supported by the driver is Groovy. In my attempt to stay on the cutting edge, I had installed ROS Hydro, the newest distro at the time of writing this post. Fortunately, Hydro and Groovy aren't very different from each other. The biggest change since Groovy was the introduction of the catkin system which replaced rosbuild. 

From a previous Groovy installation that I was no longer using, I installed the brown drivers using:

sudo apt-get install ros-groovy-brown-drivers

After installing, I copied the "irobot_create_2_1" directory from inside "/opt/ros/groovy/stacks/brown_drivers" folder to my workspace's "src" directory. For example:

cp -r /opt/ros/groovy/stacks/brown_drivers ~/catkin_ws/src

Then, in my workspace directory, I ran catkin_make, and the driver ran perfectly! To run the driver use:

rosrun irobot_create_2_1 driver.py

 Thanks to Brown driver maintainers for updating the driver to be compatible with the new catkin system, everything works just fine so far. To test the driver, you can download BumpGo.py and place it in the "irobot_create_2_1/bin" directory. To test, simply enter:

rosrun irobot_create_2_1 bumpGo.py

The robot should then drive until the bump sensor is triggered, turn and continue. My future goals are to create a simple node to drive the robot using keyboard commands, interface an Xbox Kinect, and run some kind of SLAM algorithm. I really just wanted to make sure I added some documentation as to what I've done to run the create on ROS Hydro, regardless of how simple it was. (In the very likely event that I get distracted, come back months from now to continue the project, and forget everything I learned since then)

UPDATE:
Here's a link to the create drivers package:
https://dl.dropboxusercontent.com/u/47687797/irobot_create_2_1.tar.gz

UPDATE 2:
After reinstalling ROS on a new computer without my previous groovy installation, I had some trouble getting the above package to run. When I ran "rosrun irobot_create_2_1 driver.py", I would get a message which said " the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update ". At first, the roomba would not connect and displayed some other error message I am now unable to reproduce. After replacing the battery the next morning, the create is now connecting without any problems! Let me know in the comments if you have any issues regarding this.