Program with Robot Operating System for Smooth Servo Movement

Computers & Mobile Robotics Technology
Program with Robot Operating System for Smooth Servo Movement

Robot Operating System (ROS) is an open source robotics platform that helps your robot visualize the world, map and navigate it, and perform physical interactions using state-of-the-art algorithms. If you want to build a complex robot, chances are there is some ROS code already available to help you. You can use as little of ROS as you like, and it installs on machines from the Raspberry Pi level upwards.

m55_cover_hirez
Read articles from the magazine right here on Make:. Don’t have a subscription yet? Get one today.

Let’s consider how to control a servo as an introduction to ROS. One drawback of servomotors is that they will often run as fast as they can to obey your command. This can result in your robot falling over because it suddenly started to rotate at top speed. Once we get ROS to control the servo, we can add sinusoidal-like control to keep your robot steady. You can do this in ROS without changing the controlling code, or the code that exposes the servo to ROS, or the servo hardware itself. And you can easily reuse the code for other projects in the future!

ROS has very good support for installation on Ubuntu or Debian, so you won’t have to compile to get going. This build uses a Linux machine running Ubuntu, a hobby servo, an Arduino, and a few bits of common cables like hookup wires. ROS will be running on the Ubuntu machine and its messages will be sent over USB to the Arduino. Once you have installed the binary ROS packages, let your Arduino environment know about the ROS libraries by entering the following commands in a console program (such as gnome-terminal or konsole):
cd ~/sketchbook/libraries
rm -rf ros_lib
rosrun rosserial_arduino make_l ibraries.py .

Program the Arduino

gswros_robotarm-4
Photo by Hep Svadja

Now we can upload a sketch to an Arduino to perform the low-level servo control and control it from the Linux machine. This will move a servo to a location specified as a percentage (0.0 to 1.0) of the full motion we want to allow. Using a percentage instead of an explicit angle lets the Arduino code limit the exact angle that can be set, to explicitly avoid angles that you know will cause a collision.

As you can see, the normal setup and loop functions become quite sparse when using ROS. The loop function can be the same for any Arduino code that’s just subscribing to data. In the setup you have to initialize ROS and then call subscribe for each ROS message subscriber you have. Each subscriber takes up RAM on your Arduino, so you might only have 6-12 of them depending on what else your sketch needs to do.

#include <Arduino.h>
#include <Servo.h>

#include <ros.h>
#include <std_msgs/Float32.h>

#define SERVOPIN 3

Servo servo;
void servo_cb( const std_msgs:: Float32& msg )
{
const float min = 45;
const float range = 90;
float v = msg.data;

if( v > 1 ) v = 1;
if( v < 0 ) v = 0;

float angle = min + (range * v);
servo.write(angle);
}
ros::Subscriber<std_msgs::Float 32> sub( “/head/tilt”, servo_cb );

ros::NodeHandle nh;

void setup()
{
servo.attach(SERVOPIN);

nh.initNode();
nh.subscribe(sub);
}

void loop()
{
nh.spinOnce();
delay(1); }

Now you need to be able to talk to the Arduino from the ROS world. The simplest way to do that is with a robot launch file. While the below file is very simple, these can include other launch files so you can eventually start a very complex robot with a single command.

$ cat rosservo.launch

<launch>
<node pkg="rosserial_python
" type="serial_node.py" nam
e= "osservo" respawn="true"
output="screen">
<param name="port" value=
"/dev/ttyUSB0" />
</node>
</launch>

$ roslaunch ./rosservo.lanch

The rostopic command lets you see where you can send ROS messages on your robot. As you can see below, the /head/tilt is available from the Arduino. A message can be sent using rostopic pub, the -1 option means to only publish the message once and we want to talk to /head/tilt sending a single floating point number.

$ rostopic list
/diagnostics
/head/tilt
/rosout
/rosout_agg

$ rostopic pub -1 /head/tilt std_msgs/Float32 0.4
$ rostopic pub -1 /head/tilt std_msgs/Float32 0.9

At this stage, anything that knows how to publish a number in ROS can be used to control the servo. If we move from 0 to 1 then the servo will run at full speed, which in itself is fine, but we might like the motor to accelerate to full speed and then slow down when it gets near the destination position. Less sudden motion, less jerky robot movement, less surprise to the humans in the area.

Smooth with Another Node

The below Python script listens to messages on /head/tilt/smooth and publishes many messages to /head/tilt to move the servo with a slow ramp up and a ramp down when getting close to the desired position. The moveServo_cb is called whenever a message arrives on /head/tilt/smooth. The callback then generates a number for every 10 degrees from -90 to +90 into the angles array. The sin() is taken on those angles which gives values ranging slowly from -1 to +1. Adding 1 to that makes the range 0 to +2, so a divide by 2 makes our array ramp up from 0 to +1. It’s then a matter of walking through the m array and publishing a message each time, moving slightly further through the range r each time, ending up at 1*r or the full range.

#!/usr/bin/env python

from time import sleep
import numpy as np

import rospy
from std_msgs.msg import Float32

currentPosition = 0.5
pub = None

def moveServo_cb(data):
global currentPosition, pub

targetPosition = data.data
r = targetPosition - curren
tPosition
angles = np.array( (range(1
90)) [0::10]) - 90
m = ( np.sin( angles * np.pi
/ 180. ) + 1 ) /2

for mi in np.nditer(m):
pos = currentPosition +
mi*r
print “pos: “, pos
pub.publish(pos)
sleep(0.05)

currentPosition = targetPosi
tion
print “pos-e: “, currentPos
ition
pub.publish(currentPosition)

def listener():
global pub
rospy.init_node(‘servoencod
er’, anonymous=True)
rospy.Subscriber(‘/head/til
t/smooth’, Float32, moveSer
vo_cb)
pub = rospy.Publisher(‘/h
ead/tilt’, Float32, queue_
size=10)
rospy.spin()

if __name__ == ‘__main__’:
listener()

To test out smooth servo motion, start the Python script and publish your messages to /head/tilt/smooth and you should see a smoother movement.

$ ./servoencoder.py
$ rostopic pub -1 /head/tilt/smooth std_msgs/Float32 1
$ rostopic pub -1 /head/tilt/smooth std_msgs/Float32 0

You can also remap the name of things in ROS. This way you can remap /head/tilt/smooth to be /head/tilt and the program commanding the servo will not even know that the sinusoidal motion is being used.

Going Further

I’ve focused on simple servo control here but ROS has support for much more. If you want to know what is blocking your robot from moving, there is already support for using a Kinect in ROS. Even if the navigation stack is using that data to do mapping, you can also feed a little Python script that moves a servo to track the closest object to the robot. Yes, the eyes really are following you.

Two ROS projects of mine are Terry and Houndbot. Terry is an indoor robot with two Kinects, one used exclusively for navigation, the other for depth mapping as I see fit. With its six Arduinos, Terry can be controlled via a ROS-backed web interface or directly via PS3 remote.

I designed the Houndbot for outdoor use. It has an RC remote, GPS, compass, and ROS controlled ears. I am working on getting it to use a PS4 eye twin camera for navigation. It cannot use a Kinect because the sun stops that from working. Since the hound is about 20kg I have upgraded the suspension recently, leading me to make custom alloy parts.

Robot Operating System Resources

Installation on Ubuntu
Delve into the world of navigation with ROS
ROS Q&A
Grab one of the many books on ROS
Get your robot arm on the move with ROS & MoveIt!
Run the NASA-GM Robonaut2 in a simulator. ROS is up there!

Discuss this article with the rest of the community on our Discord server!
Tagged
Ben Martin

Dr. Ben Martin wrote the libferris virtual filesystem (like plan9 for Linux), hacks on FontForge, various ODF tools, and makes robots for fun.

View more articles by Ben Martin

ADVERTISEMENT

Maker Faire Bay Area 2023 - Mare Island, CA

Escape to an island of imagination + innovation as Maker Faire Bay Area returns for its 15th iteration!

Buy Tickets today! SAVE 15% and lock-in your preferred date(s).

FEEDBACK