# User:Jryan

Robot Lab Spring 2012. I worked on robot localization, applying kalman filters to GPS measurements, and ROS setup. Code is included in-line in this Wiki page and at the bottom in a tar.

# What I Did

## Localization with the Pioneer

### Overview

p2os was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over ROS topics. rviz is used to listen to the ROS topics, and then visualize the odometery and pose information in a 3D simulation. Integration with rviz also allows future expansion to use other ROS packages and topics. For instance, the navigation package, which would allow the user to provide goals to the robot, or also visualize the map or other SLAM sensors.

### Screenshots

Going to the lab tomorrow to take some screen shots.

### How To

Start with the Wiki pages I created for setting up our robot. The pages also tell you how to start the 3D simulation for the robot's sensors.

## gps on the side

### overview

An example of a Kalman Filter, filtering GPS coordinates.

### code

```# Code adapted from:
# Kalman filter example demo in Python

# A Python implementation of the example given in pages 11-15 of "An
# Introduction to the Kalman Filter" by Greg Welch and Gary Bishop,
# University of North Carolina at Chapel Hill, Department of Computer
# Science, TR 95-041,
# http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html

# by Andrew D. Straw

class KalmanFilter:
def __init__(self, obs_func):
self.Q = 1e-5        # process variance
self.xhat=[0.0]      # a posteri estimate of x
self.P=[1.0]         # a posteri error estimate
self.xhatminus=[0.0] # a priori estimate of x
self.Pminus=[0.0]    # a priori error estimate
self.K=[0.0]         # gain or blending factor
self.R = 0.5**2      # estimate of measurement variance, change to see effect
self.obs_func = obs_func

def next_step(self):
# time update
self.xhatminus.append(self.xhat[-1])
self.Pminus.append(self.P[-1]+self.Q)
# measurement update
self.K.append(self.Pminus[-1]/( self.Pminus[-1]+self.R))
self.xhat.append(self.xhatminus[-1]+self.K[-1]*(self.obs_func()-self.xhatminus[-1]))
self.P.append((1-self.K[-1])*self.Pminus[-1])
return self.xhat[-1]

class LocationKalmanFilter():
"""This makes the assumption that x and y are independent."""

def __init__(self, x_func, y_func):
self.x_filter = KalmanFilter(x_func)
self.y_filter = KalmanFilter(y_func)

def next_step(self):
return (self.x_filter.next_step(), self.y_filter.next_step())

class GpsKalmanFilter(LocationKalmanFilter):
def __init__(self):
LocationKalmanFilter.__init__(self, self.get_long, self.get_lat)
import gps
self.session = gps.gps()

def get_long(self):
return self.session.fix.longitude

def get_lat(self):
return self.session.fix.latitude

def next_step(self):
# a = altitude, d = date/time, m=mode,
# o=postion/fix, s=status, y=satellites
LocationKalmanFilter.next_step()

# example
import pylab
import random
import time
x=[]
y=[]

xhat = []
yhat = []
kf = GpsKalmanFilter()
# take 40 measurements
for i in xrange(0, 40):
xh,yh = kf.next_step()
xhat.append(xh)
yhat.append(yh)
time.sleep(1)

# graph
pylab.figure()
pylab.plot(x,y, 'k+',label='observations',color='y')
pylab.plot(xhat, yhat, 'b-',label='a posteri estimate',color='b')
pylab.legend()
pylab.show()
```

### howto

This script takes a GPS and runs a Kalman filter over the data. The Kalman filter provides a more accurate and less erratic measurement.

1. Install gpsd. (sudo apt-get install gpsd)
2. Plug in the gps via usb,
3. Start gpsd (man gpsd to see how to use gpsd).
4. Run xgps and wait for a signal.
5. Run python kalman.py

Going to lab tomorrow to take screenshots.

## ros launch scripts

### overview

A project that launches Unicorn.

### code

```<launch>
<node pkg="p2os_driver" type="p2os" name="p2os" output="screen">
<rosparam>port: /dev/ttyUSB0</rosparam>
</node>
<node pkg="p2os_dashboard" type="p2os_dashboard" name="p2os_dashboard" output="screen">
<rosparam>port: /dev/ttyUSB0</rosparam>
</node>
<node pkg="rviz" type="rviz" name="rviz" output="screen"/>
</launch>
```

## ros patches

### overview

These patches to fixes the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are include apply these patches automatically.

### code

```Index: p2os_teleop/src/p2os_teleop.cc
===================================================================
--- p2os_teleop/src/p2os_teleop.cc	(revision 478)
+++ p2os_teleop/src/p2os_teleop.cc	(working copy)
@@ -145,7 +145,7 @@
void joy_cb(const joy::Joy::ConstPtr& joy_msg)
{

return;
@@ -154,20 +154,20 @@
last_recieved_joy_message_time_ = ros::Time::now();

// Base
-    running_ = (((unsigned int)run_button < joy_msg->get_buttons_size()) && joy_msg->buttons[run_button]);
+    running_ = (((unsigned int)run_button < joy_msg->buttons.size()) && joy_msg->buttons[run_button]);
double vx = running_ ? max_vx_run : max_vx;
double vy = running_ ? max_vy_run : max_vy;
double vw = running_ ? max_vw_run : max_vw;

-    if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy_msg->get_axes_size()))
+    if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy_msg->axes.size()))
req_vx = joy_msg->axes[axis_vx] * vx;
else
req_vx = 0.0;
-    if((axis_vy >= 0) && (((unsigned int)axis_vy) < joy_msg->get_axes_size()))
+    if((axis_vy >= 0) && (((unsigned int)axis_vy) < joy_msg->axes.size()))
req_vy = joy_msg->axes[axis_vy] * vy;
else
req_vy = 0.0;
-    if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy_msg->get_axes_size()))
+    if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy_msg->axes.size()))
req_vw = joy_msg->axes[axis_vw] * vw;
else
req_vw = 0.0;
```
```Index: p2os_teleop/manifest.xml
===================================================================
--- p2os_teleop/manifest.xml	(revision 457)
+++ p2os_teleop/manifest.xml	(working copy)
@@ -9,7 +9,7 @@
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/p2os_teleop</url>
<depend package="roscpp"/>
-	<depend package="joy"/>
+	<depend package="sensor_msgs"/>
<depend package="geometry_msgs"/>
</package>

Index: p2os_teleop/src/p2os_teleop.cc
===================================================================
--- p2os_teleop/src/p2os_teleop.cc	(revision 457)
+++ p2os_teleop/src/p2os_teleop.cc	(working copy)
@@ -52,7 +52,7 @@
#include <unistd.h>
#include <math.h>
#include "ros/ros.h"
-#include "joy/Joy.h"
+#include "sensor_msgs/Joy.h"
#include "geometry_msgs/Twist.h"

class TeleopBase
@@ -142,7 +142,7 @@
passthrough_cmd = *pass_msg;
}

-	void joy_cb(const joy::Joy::ConstPtr& joy_msg)
+	void joy_cb(const sensor_msgs::Joy::ConstPtr& joy_msg)
{

```

## ros install scripts

### overview

Install ROS and setup our robot's files from our repository.

### howto

Unzip the provided tar, it's a git repository with the entire classes' work. Go to robotlab/ros/ and run setup-ros.bash.

### code

setup-ros.bash

```#!/bin/bash -e

DIR="\$( cd "\$( dirname "\$0" )" && pwd )"

# if ros is not setup, install it
if [[ -z /etc/apt/sources.list.d/ros-latest.list || "\$1" == '-r' ]]
then
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu oneiric main" > /etc/apt/sources.list.d/ros-latest.list'
wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
sudo apt-get update -y
sudo apt-get install ros-fuerte-desktop-full ros-fuerte-joystick-drivers -y
sudo apt-get install python-pip -y
sudo pip install -U rosinstall vcstools rosdep
fi

# setup local ros env if it's not setup already
grep "source ~/robotlab/ros/bin/setup-ros-env.bash" ~/.bashrc || echo "source ~/robotlab/ros/bin/setup-ros-env.bash" >> ~/.bashrc
source ~/.bashrc

# install stacks
. \$DIR/setup-unicorn-stacks.bash

echo "Done."
```

setup-ros-env.bash

```#!/bin/bash
. /opt/ros/fuerte/setup.bash
export ROS_PACKAGE_PATH=~/robotlab/ros/stacks:\$ROS_PACKAGE_PATH
# to make rviz work in a vm
export OGRE_RTT_MODE=FBO
```

setup-unicorn-stacks.bash

```#!/bin/bash
. /opt/ros/fuerte/setup.bash
export ROS_PACKAGE_PATH=~/robotlab/ros/stacks:\$ROS_PACKAGE_PATH
# to make rviz work in a vm
export OGRE_RTT_MODE=FBO
jryan@ros:~/robotlab/ros/bin\$ cat setup-unicorn-stacks.bash
#!/bin/bash -e

DIR="\$( cd "\$( dirname "\$0" )" && pwd )"

# install p2os with our patches
echo "Checking out usc-ros-pkg from svn repo..."
rm -rf \$DIR/../stacks/usc-ros-pkg
svn co http://usc-ros-pkg.svn.sourceforge.net/svnroot/usc-ros-pkg/trunk/ \$DIR/../stacks/usc-ros-pkg
echo "Applying patch..."
pushd \$DIR/../stacks/usc-ros-pkg/p2os
patch -p0 -i ../../unicorn/p2os-teleop-fix.patch
patch -p0 -i ../../unicorn/fuerte-update.patch
popd
echo "Building..."
rospack profile && rosstack profile
set +e
sudo rosdep init
set -e
rosdep update
rosdep install p2os && rosmake p2os
rosmake register_pointclouds
echo "Done."
```

```#!/bin/bash -e

DIR="\$( cd "\$( dirname "\$0" )" && pwd )"

# install p2os with our patches
echo "Checking out usc-ros-pkg from svn repo..."
rm -rf \$DIR/../stacks/usc-ros-pkg
svn co http://usc-ros-pkg.svn.sourceforge.net/svnroot/usc-ros-pkg/trunk/ \$DIR/../stacks/usc-ros-pkg
echo "Applying patch..."
pushd \$DIR/../stacks/usc-ros-pkg/p2os
patch -p0 -i ../../unicorn/p2os-teleop-fix.patch
patch -p0 -i ../../unicorn/fuerte-update.patch
popd
echo "Building..."
rospack profile && rosstack profile
set +e
sudo rosdep init
set -e
rosdep update
rosdep install p2os && rosmake p2os
rosmake register_pointclouds
echo "Done."
#!/bin/bash -e

# problem with ~/.pip from the first script
sudo rm -rf ~/.pip

./setup-ros.bash \$@
```

## Documentation

I added these pages to the wiki to help the class install/setup ROS.

# Tips and Tutorials

## Virtual Box

### overview

Most of the class used a Virtual Box to work inside of so ROS didn't mess with our computers. There were some advantages as well as some drawbacks.

### pros and cons

Pros:

1. Isolated from host system.
2. Easy to delete and start over.

Cons:

1. Slower on some machines. New machines it was good.
2. rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
3. Kinect does NOT work through a VirtualBox.

### what not to do

If you're using a kinect with ros, as of June 2012, do NOT use VirtualBox.