Latest revision as of 23:45, 18 June 2012

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 for history's sake; I'm not sure how long the repository will stay online.

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 page I created for setting up our robot. The pages also tell you how to start the 3D simulation for the robot's sensors.

GPS with Kalman Filter

Overview

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

How To

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. Copy the code below and put it into a file called kalman.py.
6. Run python kalman.py

Screenshots

Going to lab tomorrow to take screenshots.

Code

kalman.py:

# 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
        self.session.query('admosy')
        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()

ROS Launch Script

Overview

A ROS launch script that starts p2os, and rviz.

How To

Follow the instructions on the main page.

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

I had to fix p2os (the pioneer's ROS package) a few times during the term. The repository as it was, was not working with telop and was not working with fuetre when ROS released it. The ROS setup scripts that are included in the repository apply these patches automatically.

Code

Fixes any of the telop errors that have to do with hash problems.

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)
 	{
         
-    deadman_ = (((unsigned int)deadman_button < joy_msg->get_buttons_size()) && joy_msg->buttons[deadman_button]);
+    deadman_ = (((unsigned int)deadman_button < joy_msg->buttons.size()) && joy_msg->buttons[deadman_button]);
 
     if (!deadman_)
     	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;

Updates p2os for ROS fuetre:

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)
 	{
         
     deadman_ = (((unsigned int)deadman_button < joy_msg->get_buttons_size()) && joy_msg->buttons[deadman_button]);

ROS Install Scripts

Overview

Install ROS and setups our robot's stack from our repository. Made to work on Ubuntu 11.04.

How To

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."

upgrade-ros.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."
jryan@ros:~/robotlab/ros/bin$ cat upgrade-ros.bash 
#!/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.

• ROS_(Robot_Operating_System)
• Navigation

Tips and Tricks

General Tips

• DO NOT use ROS on anything other than what it was made for: whatever version Ubuntu they are on. It's a REAL pain, and waste of time to do it any other way.
• DO NOT use VirtualBox of Ubuntu if you're going to use a Kinect (doesn't work).
• DO NOT push the pioneer robot manually, it's bad for the motor!
• DO read all the documentation for ROS: http://www.ros.org/wiki/. Read each section and you'll thank yourself later.

Virtual Box

Overview

Most of the class used a Virtual Box to setup ROS on so it didn't mess with our computers.

How To

1. $ sudo apt-get install virtualbox

2. Run virtualbox.
3. Install Ubuntu in virtual box.
4. Enabled 3D acceleration if using rviz.

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.

What Next?

Next would be to integrate all the parts the team had together into a functioning robot. It would include mostly work with ROS. Also, try taking the robot outside and testing the GPSs properly.

What would I do differently?

Have more well-defined goals for each team member and for the entire group.