Writing a custom joystick teleop node for ROS

Teleop node screenshotI’m learning a lot about ROS with every component I add to YardBot but one of the things I haven’t done yet, and will be required to do lots of, is make a custom C++ node for controlling systems. The ROS wiki has excellent documentation, and one of the better examples is how to make a node for teleoperation with a Twist message using a joystick. The plain example is a great way to learn about making C++ subscribers and publishers so I decided to start with it as my first node, with a few improvements.

The high-level design is pretty straightforward: subscribe to the joystick topic that interprets the hardware (this means you can abstract out what specific device is connected), scale the chosen linear and angular axis by a specific value and publish a Twist message on a useful topic. For a mobile robot, the resulting message is generally interpreted by a custom node that can produce direct motor commands. For now I’m using the turtlesim application to test joystick control. For improvements, I’m going to add a deadman’s switch, which is a button that must be pressed for the node to actually publish a message, and a timeout timer that zeros the output if there is no joystick message received for a set period of time.

Here is the full C++ code, and I’ll break down the different components after.

First up is class header section, where I set the public initializer along with the main private methods and variables.

Here is the main class constructor for the node, which I’ve called JoyTeleop. You can see that I’m subscribing to the /joy topic and publish the twist message on /cmd_vel (this is remapped by a launch file to connect to turtlesim). UpdateParameters is a method that queries the rosparam server and retrieves the values we need for operation: linear and angular axes index, deadman switch button index, linear and angular axes scale factors. I set all of those values with a launch file, but the code here also sets a default value if they’re not found.

This method is the primary call for generating the Twist message. First we check the status of the deadman switch button. If it’s pressed, scale the axes according to the parameters set above and publish the message. If the switch is not pressed, check the previous value and send a zero message to stop whatever it is you’re controlling. At the bottom is a call to create a timer that will send a zero message if no callback is received after 0.5 seconds (2 Hz).

Here I define the timer callback and the blank twist message generator. You’ll notice from the turtlesim example that only the forward direction and z axis rotation properties are actually required (the rest are 0 on initialization).

Finally there is the main ROS program loop, where we initialize the ROS environment, assign the node a name, construct the custom node class object and spin ROS to trigger callbacks.

A simple call to catkin_make inside the ROS workspace will build the new node, but only if you’ve set up CMakeLists properly. It took me some time to actually get my node to compile correctly, so here are the sections that may help you build the node as well. It’s not a complete CMakeLists file but contains some of the properties I had to specifically set.

Many of these values are explained in depth by the ROS wiki but I found some additional properties were required. First is the catkin_package property that sets what libraries and packages are required. I had a linker error when compiling my node, so that’s why include_directories and target_link_libraries are set. The add_executable property is the key entry, as it tells catkin to actually make your executable. Inside the call you name the node and assign whatever source code files are required.

Since this node now requires setting parameters and depends on another node, I thought it was a good time to also learn about roslaunch, which makes it easy to set properties and launch nodes together. YardBot will have multiple launch files to start the different systems but for now this component only needs to launch the joystick node and this custom one.

Above is the complete roslaunch file I’m using. It’s reasonably easy to follow along based on the wiki, especially if you’ve used XML before. The first parent is where we start the joystick node, while setting the property dev which is the hardware port the joystick is connected to. Next comes our custom node, with related properties. You’ll also see that I’m remapping /cmd_vel to /turtle1/cmd_vel so that I can test with turtlesim. If you look at the parameter values, you can also see that the left analog stick controls both linear and angular movement and the deadman switch is R1 (right top trigger).

In the scheme of robot nodes, it’s pretty simple, but you have to start somewhere. Once I’ve finalized more of the nodes I’m planning on building, I’ll be sharing that process but for now I’m just pleased I can get meaningful output from hardware. Next I’ll be trying to integrate that into a 12V motor to test the full hardware/software integration.

Posted in Software Tagged with: , , ,

Leave a Reply

Your email address will not be published. Required fields are marked *