GICLWiki - User contributions [en]

Robot Lab (Spring 2012)

2012-06-19T03:47:27Z

Jryan: /* Student Pages */

[[Category:Classes]]
== Class Resources ==
=== Mailing List ===
* [http://mail.cs.drexel.edu/pipermail/robotlab/ Archive]
* [https://mail.cs.drexel.edu/mailman/listinfo/robotlab Management]
=== Git Repository ===
<pre>
git clone ssh://<USER>@tux.cs.drexel.edu/~taw38/repos/robotlab.git
</pre>
Where <tt><USER></tt> is your Tux user name.

== Project Notes ==
* [[ROS (Robot Operating System)]] - general stuff, interfacing with the robot
* [[Navigation]]
* [[Motion and Path Planning]]
== Student Pages ==
How-to's and what we've learned:
* [[User:jryan]]
* [[User:aeh84]]
* [[User:aps43]]
* [[USer:Ws342]]

User:Jryan

2012-06-19T03:45:47Z

Jryan: /* What Next? */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==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.
<pre>
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;
</pre>

Updates p2os for ROS fuetre:
<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:45:16Z

Jryan: /* What would I do differently? */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==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.
<pre>
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;
</pre>

Updates p2os for ROS fuetre:
<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.
=What would I do differently?=
Have more well-defined goals for each team member and for the entire group.

User:Jryan

2012-06-19T03:44:59Z

Jryan: /* Tips and Tricks */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==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.
<pre>
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;
</pre>

Updates p2os for ROS fuetre:
<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.
=What would I do differently?=
* Have more well-defined goals for each team member and for the entire group.

User:Jryan

2012-06-19T03:43:24Z

Jryan: /* Overview */

User:Jryan

2012-06-19T03:42:31Z

Jryan: /* Code */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. 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.
<pre>
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;
</pre>

Updates p2os for ROS fuetre:
<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:42:13Z

Jryan: /* Code */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. 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.
<pre>
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;
</pre>

Updates p2os for ROS fuetre.
<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:41:32Z

Jryan: /* Code */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. 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.
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:40:56Z

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 for history's sake; I'm not sure how long the repository will stay online.
=What I Did=
==Localization with the Pioneer==
===Overview===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===Code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:38:00Z

Jryan: /* General Tips */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===Code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:36:57Z

Jryan: /* Tips and Tutorials */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===Code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== 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 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:36:36Z

Jryan: /* Tips */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===Code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:36:09Z

Jryan: /* Tips and Tutorials */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===Code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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 read all the documentation for ROS: http://www.ros.org/wiki/. Read each section, 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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].
===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:33:46Z

Jryan: /* ros install scripts */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===Code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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:
<pre>
#!/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."
</pre>

setup-ros-env.bash:
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash:
<pre>
#!/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 $@
</pre>

== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:32:43Z

Jryan: /* code */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===Code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:32:31Z

Jryan: /* overview */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===Overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:32:17Z

Jryan: /* overview */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix p2os (the pioneer's ROS package). The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.

===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:31:54Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:31:40Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:31:30Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 [[ROS_(Robot_Operating_System)|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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:31:03Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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:
* [[ROS_(Robot_Operating_System)]]

==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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the [[ROS_(Robot_Operating_System)|main page]].

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:29:51Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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:
* [[ROS_(Robot_Operating_System)]]

==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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page:
* [[ROS_(Robot_Operating_System)]]

===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:29:30Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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:
* [[ROS_(Robot_Operating_System)]]

==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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:29:18Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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:
* ROS_(Robot_Operating_System)

==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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:26:56Z

Jryan: /* ros */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

User:Jryan

2012-06-19T03:26:35Z

Jryan: /* what not to do */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:26:08Z

Jryan: /* pros and cons */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===Pros and Cons===
====Pros====
# Isolated from host system.
# Easy to delete and start over.
====Cons====
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:25:36Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].
# [http://maketecheasier.com/enable-3d-acceleration-in-virtualbox/2009/05/21 Enabled 3D acceleration if using rviz].

===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:24:32Z

Jryan: /* howto */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

=Tips and Tutorials=
==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===
# <pre>$ sudo apt-get install virtualbox</pre>
# Run virtualbox.
# [http://www.psychocats.net/ubuntu/virtualbox Install Ubuntu in virtual box].

===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:23:09Z

Jryan: /* overview */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

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

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:22:49Z

Jryan: /* overview */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

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

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:21:44Z

Jryan: /* Documentation */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* [[ROS_(Robot_Operating_System)]]
* [[Navigation]]

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

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:21:14Z

Jryan: /* Documentation */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
* http://gicl.cs.drexel.edu/wiki/ROS_(Robot_Operating_System)
* http://gicl.cs.drexel.edu/wiki/Navigation

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

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:20:58Z

Jryan: /* Documentation */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== Documentation ==
I added these pages to the wiki to help the class install/setup ROS.
http://gicl.cs.drexel.edu/wiki/ROS_(Robot_Operating_System)
http://gicl.cs.drexel.edu/wiki/Navigation

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

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:20:03Z

Jryan: /* ros patches */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==ROS Patches==
===overview===
I had to created patches to fix the pioneer's ROS package. The repository as it was, was not working. The ROS setup scripts that are included in the repository apply these patches automatically.
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:19:11Z

Jryan: /* ROS Launch Script= */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script==
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:18:55Z

Jryan: /* ros launch scripts */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ROS Launch Script===
===Overview===
A [http://ros.org/wiki/roslaunch ROS launch] script that starts p2os, and rviz.
===How To===
Follow the instructions on the main page.
===Code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:17:46Z

Jryan: /* How To */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Copy the code below and put it into a file called <tt>kalman.py</tt>.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:17:25Z

Jryan: /* howto */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:16:57Z

Jryan: /* gps on the side */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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 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.
===howto===
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>
===Screenshots===
Going to lab tomorrow to take screenshots.
===Code===
kalman.py:
<pre># 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()
</pre>

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

ROS (Robot Operating System)

2012-06-19T03:15:23Z

Jryan: /* rviz */

We used [http://www.ros.org ROS] so we could take advantage of the large amount of packages it provides. This is so we don't have to reinvent the wheel.
== p2os ==
p2os is the ROS stack for communicating with the pioneer robot.
An example of the pioneer robot using ROS and the navigation system: http://youtu.be/f3Iu2t0d3xo.

=== Installing ===
Open up a terminal.
Clone the git repository into your home directory:
<pre>git clone <USER>@tux.cs.drexel.edu:~taw38/repos/robotlab.git</pre>

Enter the repository:
<pre>cd ~/robotlab</pre>

Then run the ros setup script.
<pre>./ros/bin/setup-ros.bash</pre>

Wait for passwords, until you get the done prompt.

=== Control with Keyboard ===
After starting the p2os driver and everything, you can control the robot via keyboard with:
<pre>$ roslaunch p2os_launch teleop_keyboard.launch</pre>

=== rviz ===
Just launch the robot's rviz launch with (robot lab is the git repository root):
<pre>$ cd robotlab/ros/stacks/unicorn
$ roslaunch unicorn.launch</pre>

ROS (Robot Operating System)

2012-06-19T03:14:22Z

Jryan:

ROS (Robot Operating System)

2012-06-19T03:10:25Z

Jryan:

We used [http://www.ros.org ROS] so we could take advantage of the large amount of packages it provides. This is so we don't have to reinvent the wheel.
== p2os ==
p2os is the ROS stack for communicating with the pioneer robot.
An example of the pioneer robot using ROS and the navigation system: http://youtu.be/f3Iu2t0d3xo.

=== Installing ===
Open up a terminal.
Clone the git repository into your home directory:
<pre>git clone <USER>@tux.cs.drexel.edu:~taw38/repos/robotlab.git</pre>

Enter the repository:
<pre>cd ~/robotlab</pre>

Then run the ros setup script.
<pre>./ros/bin/setup-ros.bash</pre>

Wait for passwords, until you get the done prompt.

==== Control with Keyboard ====
After starting the p2os driver and everything, you can control the robot via keyboard with:
<pre>$ roslaunch p2os_launch teleop_keyboard.launch</pre>

ROS (Robot Operating System)

2012-06-19T03:05:09Z

Jryan:

We used ROS so we could take advantage of the large amount of packages it provides. This is so we don't have to reinvent the wheel:
[http://www.ros.org/wiki/ ROS Wiki]
== p2os ==
The ROS stack for hooking up to the pioneer.
Here is an example of our robot using ROS and the navigation system:
http://youtu.be/f3Iu2t0d3xo
=== Installing ===
Open up a terminal.
Clone the git repository into your home directory:
<pre>git clone <USER>@tux.cs.drexel.edu:~taw38/repos/robotlab.git</pre>

Enter the repository:
<pre>cd ~/robotlab</pre>

Then run the ros setup script.
<pre>./ros/bin/setup-ros.bash</pre>

Wait for passwords, until you get the done prompt.

==== Control with Keyboard ====
After starting the p2os driver and everything, you can control the robot via keyboard with:
<pre>$ roslaunch p2os_launch teleop_keyboard.launch</pre>
To get p2os_teleop working:
http://answers.ros.org/question/11417/datatypemd5sum-error-with-p2os_teleop

== rviz ==
To get rviz to work put this in your .bashrc:
<pre>export OGRE_RTT_MODE=FBO</pre>

User:Jryan

2012-06-19T03:02:28Z

Jryan: /* howto */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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===
<pre># 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()
</pre>

===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.
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

Going to lab tomorrow to take screenshots.

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:01:06Z

Jryan: /* screenshot/video? */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation 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.

===howto===
==gps on the side==
===overview===
An example of a Kalman Filter, filtering GPS coordinates.

===code===
<pre># 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()
</pre>

===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.
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

Going to lab tomorrow to take screenshots.

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T03:00:52Z

Jryan: /* overview */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation navigation] package, which would allow the user to provide goals to the robot, or also visualize the map or other SLAM sensors.

===screenshot/video?===
Going to the lab tomorrow to take some screen shots.

===howto===
==gps on the side==
===overview===
An example of a Kalman Filter, filtering GPS coordinates.

===code===
<pre># 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()
</pre>

===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.
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

Going to lab tomorrow to take screenshots.

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T02:59:29Z

Jryan: /* howto */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation navigation] package, which would allow the user to provide goals to the robot, or also visualize the map or other SLAM sensors.

===screenshot/video?===
Going to the lab tomorrow to take some screen shots.

===howto===
==gps on the side==
===overview===
An example of a Kalman Filter, filtering GPS coordinates.

===code===
<pre># 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()
</pre>

===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.
# Install gpsd. (sudo apt-get install gpsd)
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

Going to lab tomorrow to take screenshots.

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T02:58:49Z

Jryan: /* overview */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. [http://www.ros.org/wiki/rviz 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 [http://www.ros.org/wiki/navigation navigation] package, which would allow the user to provide goals to the robot, or also visualize the map or other SLAM sensors.

===screenshot/video?===
Going to the lab tomorrow to take some screen shots.

===howto===
==gps on the side==
===overview===
An example of a Kalman Filter, filtering GPS coordinates.

===code===
<pre># 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()
</pre>

===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.
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

Going to lab tomorrow to take screenshots.

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T02:56:42Z

Jryan: /* overview */

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===
[http://www.ros.org/wiki/p2os p2os] was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over [http://www.ros.org/wiki/Topics ROS topics]. rviz is used to listen to the ROS topics and 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.

===screenshot/video?===
Going to the lab tomorrow to take some screen shots.

===howto===
==gps on the side==
===overview===
An example of a Kalman Filter, filtering GPS coordinates.

===code===
<pre># 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()
</pre>

===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.
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

Going to lab tomorrow to take screenshots.

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===

User:Jryan

2012-06-19T02:56:09Z

Jryan: /* overview */

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 [http://www.ros.org/wiki/Topics ROS topics]. rviz is used to listen to the ROS topics and 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.

===screenshot/video?===
Going to the lab tomorrow to take some screen shots.

===howto===
==gps on the side==
===overview===
An example of a Kalman Filter, filtering GPS coordinates.

===code===
<pre># 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()
</pre>

===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.
# Plug in the gps via usb,
# Start gpsd (<tt>man gpsd</tt> to see how to use gpsd).
# Run <tt>xgps</tt> and wait for a signal.
# Run <tt>python kalman.py</tt>

Going to lab tomorrow to take screenshots.

==ros launch scripts==
===overview===
A project that launches Unicorn.
===howto===
===code===
<pre>
<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>
</pre>

==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.
===howto===
===code===
<pre>
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;
</pre>

<pre>
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]);
</pre>

==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
<pre>
#!/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."
</pre>

setup-ros-env.bash
<pre>
#!/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
</pre>

setup-unicorn-stacks.bash
<pre>
#!/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."
</pre>

upgrade-ros.bash
<pre>
#!/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 $@
</pre>
== 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.

===howto===
===pros and cons===
Pros:
# Isolated from host system.
# Easy to delete and start over.
Cons:
# Slower on some machines. New machines it was good.
# rviz (3D robot visualizer) had some issues (window black spots, had to debug it first to get it to work)
# 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.

==ros==
===overview===
===howto===
===pros and cons===
===what not to do===