Commit 4494a679 authored by mpurvis@clearpathrobotics.com's avatar mpurvis@clearpathrobotics.com
Browse files

Reorganizing our stacks a little, publishing some new demos for

Husky and TurtleBot.



git-svn-id: https://clearpath-ros-pkg.googlecode.com/svn/trunk/clearpath_husky@13 40067977-d5ad-3dcc-edb9-687ab173df3d
parents
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
# directories (or patterns, but directories should suffice) that should
# be excluded from the distro. This is not the place to put things that
# should be ignored everywhere, like "build" directories; that happens in
# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
# ready for inclusion in a distro.
#
# This list is combined with the list in rosbuild/rosbuild.cmake. Note
# that CMake 2.6 may be required to ensure that the two lists are combined
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
rosbuild_make_distribution(0.1.0)
include $(shell rospack find mk)/cmake_stack.mk
\ No newline at end of file
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
/**
\mainpage
\htmlinclude manifest.html
\b husky_bringup is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/
<package>
<description brief="husky_bringup">
This package provides a launch file to bring up the Husky
hardware interface in a way that will be accessible and
usable by the other software in husky_teleop and husky_kinect.
In the future, a script will be provided to allow this to
be controlled by the service command and launched on startup
of your machine.
</description>
<author>Mike Purvis</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/husky_bringup</url>
<depend package="clearpath_bringup"/>
<depend package="clearpath_base"/>
</package>
<launch>
<node pkg="clearpath_base" type="kinematic.py" name="clearpath_base">
<!-- Uncomment the following line to manually specify serial device, rather
than autodetecting between available /dev/ttyUSB and /dev/ttyS devices.
-->
<!-- <param name="port" value="/dev/ttyUSB0" /> -->
<param name="data/system_status" value="10" />
<param name="data/safety_status" value="10" />
<param name="data/encoders" value="10" />
<param name="data/differential_speed" value="10" />
<param name="data/differential_output" value="10" />
<param name="data/power_status" value="1" />
</node>
</launch>
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
<launch>
<arg name="robot_name" default="default" />
<group ns="clearpath">
<group ns="robots/$(arg robot_name)">
<!-- HUSKY -->
<include file="$(find husky_bringup)/minimal.launch" />
<!-- KINECT -->
<arg name="debug" default="false"/>
<arg if="$(arg debug)" name="launch_prefix" value="xterm -rv -e gdb -ex run -args"/>
<arg unless="$(arg debug)" name="launch_prefix" value=""/>
<node pkg="openni_camera" type="openni_node" name="openni_node1" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="device_id" value="#1"/> <!-- this line uses first enumerated device -->
<rosparam command="load" file="$(find openni_camera)/info/openni_params.yaml" />
<param name="rgb_frame_id" value="/openni_rgb_optical_frame" />
<param name="depth_frame_id" value="/openni_depth_optical_frame" />
<param name="use_indices" value="false" />
<param name="depth_registration" value="true" />
<param name="image_mode" value="2" />
<param name="depth_mode" value="2" />
<param name="debayering" value="2" />
<param name="depth_time_offset" value="0" />
<param name="image_time_offset" value="0" />
</node>
<include file="$(find openni_camera)/launch/kinect_frames.launch"/>
<!-- SKIM CLOUD -->
<node pkg="clearpath_tools" type="SkimImage" name="skim1" >
<param name="in" value="camera/rgb/points" />
<param name="out" value="camera/points/skim" />
<param name="skim_factor" value="2" />
</node>
<!-- PASSTHROUGH -->
<node pkg="clearpath_tools" type="Passthrough" name="passthrough1" >
<param name="in" value="camera/points/skim" />
<param name="out" value="camera/points/passthrough" />
<param name="dist" value="4.0" />
</node>
<!-- EXPLORE DEMO -->
<node pkg="clearpath_kinect_demo" type="ClearpathDemoExplore" name="ClearpathDemoExplore1" output="screen">
<param name="in" value="camera/points/passthrough" />
<param name="out" value="camera/points/visualization" />
<param name="out_vel" value="cmd_vel" />
<param name="angular_vel_correction" value="2" />
<param name="lin_speed" value="0.3" />
<param name="ang_speed" value="0.3" />
<param name="cam_height" value="0.55" />
<param name="cam_z_trans" value="0.35" />
<param name="wall_buffer" value="0.6" />
<param name="random_walk" value="0" />
<param name="publish_visualization" value="1" />
</node>
</group> <!-- end of robot group -->
</group> <!-- end of clearpath group -->
</launch>
/**
\mainpage
\htmlinclude manifest.html
\b husky_kinect is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/
<package>
<description brief="husky_kinect">
This package contains launch files which run some of the demos
from the clearpath_kinect stack on the Husky platform.
</description>
<author>Mike Purvis</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/husky_kinect</url>
<depends stack="clearpath_kinect" />
</package>
<launch>
<arg name="robot_name" default="default" />
<group ns="clearpath">
<group ns="robots/$(arg robot_name)">
<!-- HUSKY -->
<include file="$(find husky_bringup)/minimal.launch" />
<!-- KINECT -->
<arg name="debug" default="false"/>
<arg if="$(arg debug)" name="launch_prefix" value="xterm -rv -e gdb -ex run -args"/>
<arg unless="$(arg debug)" name="launch_prefix" value=""/>
<node pkg="openni_camera" type="openni_node" name="openni_node1" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="device_id" value="#1"/>
<rosparam command="load" file="$(find openni_camera)/info/openni_params.yaml" />
<param name="rgb_frame_id" value="/openni_rgb_optical_frame" />
<param name="depth_frame_id" value="/openni_depth_optical_frame" />
<param name="use_indices" value="false" />
<param name="depth_registration" value="true" />
<param name="image_mode" value="2" />
<param name="depth_mode" value="2" />
<param name="debayering" value="2" />
<param name="depth_time_offset" value="0" />
<param name="image_time_offset" value="0" />
</node>
<include file="$(find openni_camera)/launch/kinect_frames.launch"/>
<!-- SKIM CLOUD -->
<node pkg="clearpath_tools" type="SkimImage" name="skim1" >
<param name="in" value="camera/rgb/points" />
<param name="out" value="camera/points/skim" />
<param name="skim_factor" value="2" />
</node>
<!-- PASSTHROUGH -->
<node pkg="clearpath_tools" type="Passthrough" name="passthrough1" >
<param name="in" value="camera/points/skim" />
<param name="out" value="camera/points/passthrough" />
<param name="dist" value="4.0" />
</node>
<!-- TRACKING DEMO -->
<node pkg="clearpath_kinect_demo" type="ClearpathDemoTrack" name="ClearpathDemoTrack1" output="screen">
<param name="in" value="camera/points/passthrough" />
<param name="out" value="camera/points/visualization" />
<param name="out_vel" value="cmd_vel" />
<param name="angular_vel_correction" value="2" />
<param name="lin_speed" value="0.3" />
<param name="ang_speed" value="0.2" />
<param name="cam_height" value="0.55" />
<param name="cam_z_trans" value="0.35" />
<param name="window_size" value="0.5" />
<param name="publish_visualization" value="1" />
</node>
</group> <!-- end of robot group -->
</group> <!-- end of clearpath group -->
</launch>
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
/**
\mainpage
\htmlinclude manifest.html
\b husky_localization is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/
<package>
<description brief="husky_localization">
husky_localization
</description>
<author>Mike Purvis</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/husky_localization</url>
<depend package="rospy"/>
<depend package="geometry_msgs"/>
</package>
#!/usr/bin/env python
# Lat Long - UTM, UTM - Lat Long conversions
from math import pi, sin, cos, tan, sqrt
#LatLong- UTM conversion..h
#definitions for lat/long to UTM and UTM to lat/lng conversions
#include <string.h>
_deg2rad = pi / 180.0
_rad2deg = 180.0 / pi
_EquatorialRadius = 2
_eccentricitySquared = 3
_ellipsoid = [
# id, Ellipsoid name, Equatorial Radius, square of eccentricity
# first once is a placeholder only, To allow array indices to match id numbers
[ -1, "Placeholder", 0, 0],
[ 1, "Airy", 6377563, 0.00667054],
[ 2, "Australian National", 6378160, 0.006694542],
[ 3, "Bessel 1841", 6377397, 0.006674372],
[ 4, "Bessel 1841 (Nambia] ", 6377484, 0.006674372],
[ 5, "Clarke 1866", 6378206, 0.006768658],
[ 6, "Clarke 1880", 6378249, 0.006803511],
[ 7, "Everest", 6377276, 0.006637847],
[ 8, "Fischer 1960 (Mercury] ", 6378166, 0.006693422],
[ 9, "Fischer 1968", 6378150, 0.006693422],
[ 10, "GRS 1967", 6378160, 0.006694605],
[ 11, "GRS 1980", 6378137, 0.00669438],
[ 12, "Helmert 1906", 6378200, 0.006693422],
[ 13, "Hough", 6378270, 0.00672267],
[ 14, "International", 6378388, 0.00672267],
[ 15, "Krassovsky", 6378245, 0.006693422],
[ 16, "Modified Airy", 6377340, 0.00667054],
[ 17, "Modified Everest", 6377304, 0.006637847],
[ 18, "Modified Fischer 1960", 6378155, 0.006693422],
[ 19, "South American 1969", 6378160, 0.006694542],
[ 20, "WGS 60", 6378165, 0.006693422],
[ 21, "WGS 66", 6378145, 0.006694542],
[ 22, "WGS-72", 6378135, 0.006694318],
[ 23, "WGS-84", 6378137, 0.00669438]
]
#Reference ellipsoids derived from Peter H. Dana's website-
#http://www.utexas.edu/depts/grg/gcraft/notes/datum/elist.html
#Department of Geography, University of Texas at Austin
#Internet: pdana@mail.utexas.edu
#3/22/95
#Source
#Defense Mapping Agency. 1987b. DMA Technical Report: Supplement to Department of Defense World Geodetic System
#1984 Technical Report. Part I and II. Washington, DC: Defense Mapping Agency
#def LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long,
# double &UTMNorthing, double &UTMEasting, char* UTMZone)
def LLtoUTM(ReferenceEllipsoid, Lat, Long, zone = None):
a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
k0 = 0.9996
#Make sure the longitude is between -180.00 .. 179.9
LongTemp = (Long+180)-int((Long+180)/360)*360-180 # -180.00 .. 179.9
LatRad = Lat*_deg2rad
LongRad = LongTemp*_deg2rad
if zone is None:
ZoneNumber = int((LongTemp + 180)/6) + 1
else:
ZoneNumber = zone
if Lat >= 56.0 and Lat < 64.0 and LongTemp >= 3.0 and LongTemp < 12.0:
ZoneNumber = 32
# Special zones for Svalbard
if Lat >= 72.0 and Lat < 84.0:
if LongTemp >= 0.0 and LongTemp < 9.0:ZoneNumber = 31
elif LongTemp >= 9.0 and LongTemp < 21.0: ZoneNumber = 33
elif LongTemp >= 21.0 and LongTemp < 33.0: ZoneNumber = 35
elif LongTemp >= 33.0 and LongTemp < 42.0: ZoneNumber = 37
LongOrigin = (ZoneNumber - 1)*6 - 180 + 3 #+3 puts origin in middle of zone
LongOriginRad = LongOrigin * _deg2rad
#compute the UTM Zone from the latitude and longitude
UTMZone = "%d%c" % (ZoneNumber, _UTMLetterDesignator(Lat))
eccPrimeSquared = (eccSquared)/(1-eccSquared)
N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad))
T = tan(LatRad)*tan(LatRad)
C = eccPrimeSquared*cos(LatRad)*cos(LatRad)
A = cos(LatRad)*(LongRad-LongOriginRad)
M = a*((1
- eccSquared/4
- 3*eccSquared*eccSquared/64
- 5*eccSquared*eccSquared*eccSquared/256)*LatRad
- (3*eccSquared/8
+ 3*eccSquared*eccSquared/32
+ 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
+ (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
- (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad))
UTMEasting = (k0*N*(A+(1-T+C)*A*A*A/6
+ (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
+ 500000.0)
UTMNorthing = (k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+ (61
-58*T
+T*T
+600*C
-330*eccPrimeSquared)*A*A*A*A*A*A/720)))
if Lat < 0:
UTMNorthing = UTMNorthing + 10000000.0; #10000000 meter offset for southern hemisphere
return (UTMZone, UTMEasting, UTMNorthing)
def _UTMLetterDesignator(Lat):
if 84 >= Lat >= 72: return 'X'
elif 72 > Lat >= 64: return 'W'
elif 64 > Lat >= 56: return 'V'
elif 56 > Lat >= 48: return 'U'
elif 48 > Lat >= 40: return 'T'
elif 40 > Lat >= 32: return 'S'
elif 32 > Lat >= 24: return 'R'
elif 24 > Lat >= 16: return 'Q'
elif 16 > Lat >= 8: return 'P'
elif 8 > Lat >= 0: return 'N'
elif 0 > Lat >= -8: return 'M'
elif -8> Lat >= -16: return 'L'
elif -16 > Lat >= -24: return 'K'
elif -24 > Lat >= -32: return 'J'
elif -32 > Lat >= -40: return 'H'
elif -40 > Lat >= -48: return 'G'
elif -48 > Lat >= -56: return 'F'
elif -56 > Lat >= -64: return 'E'
elif -64 > Lat >= -72: return 'D'
elif -72 > Lat >= -80: return 'C'
else: return 'Z' # if the Latitude is outside the UTM limits
#void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
# double& Lat, double& Long )
def UTMtoLL(ReferenceEllipsoid, northing, easting, zone):
k0 = 0.9996
a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared))
#NorthernHemisphere; //1 for northern hemispher, 0 for southern
x = easting - 500000.0 #remove 500,000 meter offset for longitude
y = northing
ZoneLetter = zone[-1]
ZoneNumber = int(zone[:-1])
if ZoneLetter >= 'N':
NorthernHemisphere = 1 # point is in northern hemisphere
else:
NorthernHemisphere = 0 # point is in southern hemisphere
y -= 10000000.0 # remove 10,000,000 meter offset used for southern hemisphere
LongOrigin = (ZoneNumber - 1)*6 - 180 + 3 # +3 puts origin in middle of zone
eccPrimeSquared = (eccSquared)/(1-eccSquared)
M = y / k0
mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256))
phi1Rad = (mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
+ (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
+(151*e1*e1*e1/96)*sin(6*mu))
phi1 = phi1Rad*_rad2deg;
N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad))
T1 = tan(phi1Rad)*tan(phi1Rad)
C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad)
R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5)
D = x/(N1*k0)
Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720)
Lat = Lat * _rad2deg
Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
*D*D*D*D*D/120)/cos(phi1Rad)
Long = LongOrigin + Long * _rad2deg
return (Lat, Long)
if __name__ == '__main__':
(z, e, n) = LLtoUTM(23, 45.00, -75.00)
print z, e, n
print UTMtoLL(23, e, n, z)