felix-kolbe · bajo · Jun 17, 2014 · Nov 21, 2016
diff --git a/uashh_smach/CMakeLists.txt b/uashh_smach/CMakeLists.txt
@@ -1,30 +1,20 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html
+# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html
+cmake_minimum_required(VERSION 2.8.3)
+project(uashh_smach)
 
-# 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()
+# Load catkin and all dependencies required for this package
+# TODO: remove all from COMPONENTS that are not catkin packages.
+find_package(catkin REQUIRED COMPONENTS rospy smach_ros tf actionlib std_msgs move_base_msgs nav_msgs)
 
-#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 the package has a setup.py
+catkin_python_setup()
 
-#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})
+# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package
+catkin_package(
+    DEPENDS rospy smach_ros tf actionlib std_msgs move_base_msgs nav_msgs 
+    CATKIN_DEPENDS # TODO
+    INCLUDE_DIRS # TODO include
+    LIBRARIES # TODO
+)
diff --git a/uashh_smach/Makefile b/uashh_smach/Makefile
diff --git a/uashh_smach/manifest.xml b/uashh_smach/manifest.xml
diff --git a/uashh_smach/package.xml b/uashh_smach/package.xml
@@ -0,0 +1,35 @@
+<package>
+  <name>uashh_smach</name>
+  <version>1.0.0</version>
+  <description>Smach prototypes of University of Applied Sciences Hamburg.</description>
+  <maintainer email="[email protected]">Felix</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/uashh_smach</url>
+  <!-- <url type="bugtracker"></url> -->
+
+  <author>Felix</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>rospy</build_depend>
+  <build_depend>smach_ros</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>move_base_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>rospy</run_depend>
+  <run_depend>smach_ros</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>move_base_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+
+</package>
diff --git a/uashh_smach/setup.py b/uashh_smach/setup.py
@@ -0,0 +1,12 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=['uashh_smach'],
+    package_dir={'': 'src'},
+)
+
+setup(**setup_args)
diff --git a/uashh_smach/src/uashh_smach/platform/move_base.py b/uashh_smach/src/uashh_smach/platform/move_base.py
@@ -173,7 +173,6 @@ def _msg_cb(self, msg, ud):
 
 
 
-
 class HasMovedState(State):
     """Return whether the robot moved beyond a given minimum distance in a given frame
     since the last exceeding check.

diff --git a/uashh_smach/src/uashh_smach/tasks/task_move_around.py b/uashh_smach/src/uashh_smach/tasks/task_move_around.py
@@ -17,7 +17,6 @@
 
 def get_move_around_smach():
     sm = StateMachine(outcomes=['preempted'])
-    #sm.set_initial_state('CHECK_MOVEMENT')
 
     with sm:
         StateMachine.add('SLEEP_UNTIL_ENABLED', util.get_sleep_until_smach_enabled_smach(),
@@ -27,9 +26,6 @@ def get_move_around_smach():
                          transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
                                       'aborted':'SLEEP_UNTIL_ENABLED'})
 
-#        StateMachine.add("ARM_LOOK_AROUND", look_around.get_lookaround_smach(util.SleepState(LOOKAROUND_SLEEP_DURATION)),
-#        StateMachine.add('ARM_LOOK_AROUND', util.SleepState(1),    # mockup
-
     return sm
 
 
@@ -41,4 +37,3 @@ def main():
 
 if __name__ == '__main__':
     main()
-
diff --git a/uashh_smach/src/uashh_smach/util.py b/uashh_smach/src/uashh_smach/util.py
@@ -16,13 +16,9 @@
 
 import rostopic
 
-
-
 TAU = math.pi * 2   # one tau is one turn. simply as that.
 
 
-
-
 class PromptState(smach.State):
     """Prompt and wait for user action or input on command line.
 
@@ -276,6 +272,7 @@ def get_current_robot_position(frame='/map'):
     frame: defaults to /map
     """
     try:
+        TransformListenerSingleton.get().waitForTransform(frame, '/base_link', rospy.Time(0), rospy.Duration(4.0))
         trans, rot = TransformListenerSingleton.get().lookupTransform(frame, '/base_link', rospy.Time(0))
         (_roll, _pitch, yaw) = tf.transformations.euler_from_quaternion(rot)
         return trans[0], trans[1], yaw