Servo Control (ROS and Arduino)

Summary

In order to localize our RMC robot, we will use a camera to locate ARUCO markers that are glued to the dumping bin. Basically, they're like QR codes that we will see on the camera and figure out where we are relative to them.

The current design involves mounting a camera on a servo (so it rotates to search for the visual markers). Our software system uses ROS and it runs on the computers (laptop, or ARM boards, etc.) These can't directly control the servos. So I'm interfacing ROS and the servo using a Teensy 3.0 (basically, a super-beefed-up Arduino).

I'm using the rosserial_arduino library to interface with the Teensy. Essentially, what happens is that the Teensy acts like a ROS node, and it can send and receive most kinds of ROS messages (at least the basic ones). All you have to do (after installation and stuff) is to write your code in the Arduino IDE (it's like half-ROS half-Arduino code, i.e. it uses both libraries at the same time), and then upload it to the Teensy. Once the Teensy is running and connected over USB port, you run the rosserial node to translate what the Teensy is saying into standard ROS messages. Now, it's as if your Teensy is a real ROS node sending/receiving ROS messages.

How do I get started coding for Arduino/Teensy with ROS?

• First you need to install the Arduino IDE and the Teensyduino library as described here.
• Now set up rosserial_arduino. Note that installation using binaries (section 3.1) will not work according to this ROS answer.
• Now you can plug in the Teensy and upload some code to it (you might have to manually press the button during upload).
• Once the Teensy is plugged in and has the code uploaded to it, you need to run the rosserial node so it can relay information from the Teensy to ROS: rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0.

Tasks

• Instead of an Arduino, I'm attempting to use a Teensy 3.0, since it's mostly compatible with Arduino. Confirmed working with rosserial.
• Simple servo controller (ADC) with feedback (hacked from linear potentiometer in servo). We decided to go without feedback, because we can keep the servo motion increments (sweep-like) small enough so that we assume our servo position is the same as the command we give it.
• incorporate feedback to broadcast transform for camera frame
• Subscribe to ARUCO transform message to determine how to rotate servo.

Specs for teensy node

Subscribed Topics

• /transform from ARUCO node of type geometry_msgs/TransformStamped:
  • This transform message contains the tf from camera to the ARUCO board. The node calculates the yaw angle error using arctan(x/z), where x and z are obtained from the transform message. If the angle error is greater than some threshold, then turn the servo in that direction.

Published topics:

• /tf:
  • parent: servo_mount
  • child: camera
• /servo_camera_state of type std_msgs::Bool:
  • The message is true when the teensy has lost the aruco marker and false when it finds it.
  • Sends once upon state change (does not keep sending same value)

Summary

A rosserial node that subscribes to aruco transform node and turns servo towards it, while constantly broadcasting its tf (between servo_mount and camera on the servo).

Overall Setup

A teensy is connected to a Linux-box running ROS over USB cable, and three wires going to a servo. The servo is fixed to the robot, and a camera is mounted on the servo (the rotating part). The camera sends camera data to the Linux-box, which uses ar_sys to detect aruco markers, and then it publishes a transform message (indicating relative pose from camera to marker). This node uses that transform to calculate how far off the marker is from the camera (yaw angle only), and decides to turn in the direction of the marker. This teensy node is programmed in Arduino (for Teensy 3.1) and interfaces with ROS using the rosserial_python serial_node.

Behavior

If this teensy node fails to see the marker for a short period (SWEEP_TIMER_LIMIT), it starts sweeping the servo back and forth. If the marker is still not seen for a longer period (LOST_TIMER_LIMIT), the node declares itself lost and publishes a true value on the lost topic once. Whenever the node sees a marker again, it resets both timers and publishes a false value on the lost topic once. As long as the node is not sweeping or lost, it will turn towards the aruco marker.

How to run the Teensy node

• copy the udev rules found in the misc folder into your udev location (usually /etc/udev/rules.d/).
• Restart your udev service: sudo service udev restart.
• Plug 'er in! (the Teensy, I mean). It should show up as device /dev/serial/by-id/usb-Teensyduino_USB_Serial_840430-if00.
• Run the launch file teensy-rosserial.launch. This will run the rosserial node (which will send data back and forth between ROS and the teensy).
• You should be able to see the teensy node broadcasting a /tf (from camera to servo_mount), and publishing a /servo_camera_state (and subscribing to /ar_single_board/transform.

Miscellaneous Tips

• If you have trouble uploading the code to the Teensy, and it complains about the board not responding to USB-based request (even if you're pressing the button), just make sure your permissions when running the Arduino program are good (just use sudo as a hack).
• If the rosserial node complains about not being able to sync with the device, just make sure your node executes the spin (you can't omit that).

TODO

• fix stupid udev rules so that I don't have to use /dev/ttyACM0 every time (does it change based on host device?)
• fix permission error so I don't need to run Arduino using sudo
• handle multiple ARUCO boards
• consider delay before broadcasting tf (to make sure servo reaches that position before broadcasting)
• fix inconsistent Timer1 interrupt value. This might be an issue with rosserial or with Servo library. (using IntervalTimer instead)
• stop rotating upon immediate loss of aruco board (two-stage lost_callback). This is to minimize movement so we can re-find the aruco target. If we still can't find it, we go back to sweep. implemented two-stage mode with it first sweeping and then publishing lost message after a longer period.