Difference between revisions of "User:Jryan"

From GICL Wiki
Jump to: navigation, search
(howto)
(overview)
Line 6: Line 6:
 
==Localization with the Pioneer==
 
==Localization with the Pioneer==
 
===overview===
 
===overview===
We used p2os to interface with the robot. p2os publishes relevant odometery and pose information over ROS. I took this information setup it up to be visualized in ROS, so we can see where the robot thinks it is at any given time. Integration with rviz also allows future expansion to use other ROS package, like the navigation package, to even give the robot goals, or visualize the map and other SLAM sensors.
+
p2os was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over ROS topics. rviz is used to listen to the ROS topics and visualize the odomentry 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?===
 
===screenshot/video?===

Revision as of 21:51, 18 June 2012

Robot Lab Spring 2012. Working on Localization, GPS and ROS. Code is included on the wiki and at the bottom in a tar.

Contents

What I Did

Localization with the Pioneer

overview

p2os was used to control and receive data from the robot. p2os publishes odometery and pose information from the robot over ROS topics. rviz is used to listen to the ROS topics and visualize the odomentry 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

# 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()

howto

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

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

Going to lab tomorrow to take screenshots.

ros launch scripts

overview

A project that launches Unicorn.

howto

code

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

ros patches

overview

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

howto

code

Index: p2os_teleop/src/p2os_teleop.cc
===================================================================
--- p2os_teleop/src/p2os_teleop.cc	(revision 478)
+++ p2os_teleop/src/p2os_teleop.cc	(working copy)
@@ -145,7 +145,7 @@
 	void joy_cb(const joy::Joy::ConstPtr& joy_msg)
 	{
         
-    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;
Index: p2os_teleop/manifest.xml
===================================================================
--- p2os_teleop/manifest.xml	(revision 457)
+++ p2os_teleop/manifest.xml	(working copy)
@@ -9,7 +9,7 @@
 	<review status="unreviewed" notes=""/>
 	<url>http://ros.org/wiki/p2os_teleop</url>
 	<depend package="roscpp"/>
-	<depend package="joy"/>
+	<depend package="sensor_msgs"/>
 	<depend package="geometry_msgs"/>
 </package>
 
Index: p2os_teleop/src/p2os_teleop.cc
===================================================================
--- p2os_teleop/src/p2os_teleop.cc	(revision 457)
+++ p2os_teleop/src/p2os_teleop.cc	(working copy)
@@ -52,7 +52,7 @@
 #include <unistd.h>
 #include <math.h>
 #include "ros/ros.h"
-#include "joy/Joy.h"
+#include "sensor_msgs/Joy.h"
 #include "geometry_msgs/Twist.h"
 
 class TeleopBase 
@@ -142,7 +142,7 @@
 		passthrough_cmd = *pass_msg;
 	}
 
-	void joy_cb(const joy::Joy::ConstPtr& joy_msg)
+	void joy_cb(const sensor_msgs::Joy::ConstPtr& joy_msg)
 	{
         
     deadman_ = (((unsigned int)deadman_button < joy_msg->get_buttons_size()) && joy_msg->buttons[deadman_button]);

ros install scripts

overview

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

howto

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

code

setup-ros.bash

#!/bin/bash -e

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

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

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

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

echo "Done."

setup-ros-env.bash

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

setup-unicorn-stacks.bash

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

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

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

upgrade-ros.bash

#!/bin/bash -e

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

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

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

./setup-ros.bash $@

Documentation

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

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:

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

Cons:

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

what not to do

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

ros

overview

howto

pros and cons

what not to do