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

CMakeLists.txt

+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)

Makefile

+include $(shell rospack find mk)/cmake_stack.mk
\ No newline at end of file

husky_bringup/CMakeLists.txt

+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})

husky_bringup/Makefile

+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file

husky_bringup/mainpage.dox

+/**
+\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.
+-->
+
+
+*/

husky_bringup/manifest.xml

+<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>
+
+

husky_bringup/minimal.launch

+<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>

husky_kinect/CMakeLists.txt

+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})

husky_kinect/Makefile

+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file

husky_kinect/explore.launch

+<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>

husky_kinect/mainpage.dox

+/**
+\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.
+-->
+
+
+*/

husky_kinect/manifest.xml

+<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>
+
+

husky_kinect/track_person.launch

+<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>

husky_localization/CMakeLists.txt

+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})

husky_localization/Makefile

+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file

husky_localization/mainpage.dox

+/**
+\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.
+-->
+
+
+*/

husky_localization/manifest.xml

+<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>
+
+

husky_localization/src/LatLongUTMconversion.py

+#!/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)