Merge remote branch 'origin-ipa-tys/master' into automerge

ipa-tys · May 25, 2012 · 53846a9 · 53846a9
2 parents 43c6876 + 0dd2648
commit 53846a9
Show file tree

Hide file tree

Showing 47 changed files with 2,529 additions and 581 deletions.
diff --git a/cob_ehealth2012/.cproject b/cob_ehealth2012/.cproject
diff --git a/cob_ehealth2012/.project b/cob_ehealth2012/.project
@@ -0,0 +1,116 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>ehealth2012-RelWithDebInfo@ehealth2012</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.python.pydev.PyDevBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.make.core.makeBuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+				<dictionary>
+					<key>org.eclipse.cdt.core.errorOutputParser</key>
+					<value>org.eclipse.cdt.core.MakeErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GLDErrorParser;</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.append_environment</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.arguments</key>
+					<value></value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.command</key>
+					<value>/usr/bin/make</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.location</key>
+					<value>/home/tys/git/care-o-bot/cob_scenarios/ehealth2012</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.target.auto</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.target.clean</key>
+					<value>clean</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.target.inc</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.buildArguments</key>
+					<value></value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.buildLocation</key>
+					<value>/home/tys/git/care-o-bot/cob_scenarios/ehealth2012</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
+					<value>clean</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.contents</key>
+					<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
+					<value>false</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableFullBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enabledIncrementalBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.environment</key>
+					<value>VERBOSE=1|ROS_ROOT=/opt/ros/electric/ros|ROS_PACKAGE_PATH=/home/tys/git/care-o-bot:/home/tys/git/pr2:/home/tys/eclipse/runtime-BRIDE:/opt/ros/electric/stacks:/home/tys/brics|PYTHONPATH=/opt/ros/electric/ros/core/roslib/src:/opt/ros/electric/ros/core/roslib/src:/opt/ros/electric/ros/core/roslib/src:|PATH=/opt/ros/electric/ros/bin:/opt/ros/electric/ros/bin:/opt/ros/electric/ros/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/home/tys/brics/sbin:/home/tys/brics/sbin:/home/tys/brics/sbin:/home/tys/eclipse|</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.stopOnError</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
+					<value>false</value>
+				</dictionary>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.make.core.ScannerConfigBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.make.core.makeNature</nature>
+		<nature>org.eclipse.cdt.make.core.ScannerConfigNature</nature>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.python.pydev.pythonNature</nature>
+	</natures>
+</projectDescription>
diff --git a/cob_ehealth2012/.pydevproject b/cob_ehealth2012/.pydevproject
diff --git a/cob_ehealth2012/CMakeLists.txt b/cob_ehealth2012/CMakeLists.txt
@@ -0,0 +1,30 @@
+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})
diff --git a/cob_ehealth2012/Makefile b/cob_ehealth2012/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
diff --git a/cob_ehealth2012/cmake_install.cmake b/cob_ehealth2012/cmake_install.cmake
@@ -0,0 +1,44 @@
+# Install script for directory: /home/tys/git/care-o-bot/cob_scenarios/ehealth2012
+
+# Set the install prefix
+IF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+  SET(CMAKE_INSTALL_PREFIX "/usr/local")
+ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+  IF(BUILD_TYPE)
+    STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+  ELSE(BUILD_TYPE)
+    SET(CMAKE_INSTALL_CONFIG_NAME "RelWithDebInfo")
+  ENDIF(BUILD_TYPE)
+  MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+
+# Set the component getting installed.
+IF(NOT CMAKE_INSTALL_COMPONENT)
+  IF(COMPONENT)
+    MESSAGE(STATUS "Install component: \"${COMPONENT}\"")
+    SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+  ELSE(COMPONENT)
+    SET(CMAKE_INSTALL_COMPONENT)
+  ENDIF(COMPONENT)
+ENDIF(NOT CMAKE_INSTALL_COMPONENT)
+
+# Install shared libraries without execute permission?
+IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+  SET(CMAKE_INSTALL_SO_NO_EXE "1")
+ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+
+IF(CMAKE_INSTALL_COMPONENT)
+  SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
+ELSE(CMAKE_INSTALL_COMPONENT)
+  SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
+ENDIF(CMAKE_INSTALL_COMPONENT)
+
+FILE(WRITE "/home/tys/git/care-o-bot/cob_scenarios/ehealth2012/${CMAKE_INSTALL_MANIFEST}" "")
+FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES})
+  FILE(APPEND "/home/tys/git/care-o-bot/cob_scenarios/ehealth2012/${CMAKE_INSTALL_MANIFEST}" "${file}\n")
+ENDFOREACH(file)
diff --git a/cob_ehealth2012/config/cob3-3/arm_joint_configurations.yaml b/cob_ehealth2012/config/cob3-3/arm_joint_configurations.yaml
diff --git a/cob_ehealth2012/config/ipa-apartment/navigation_goals.yaml b/cob_ehealth2012/config/ipa-apartment/navigation_goals.yaml
@@ -0,0 +1,7 @@
+# navigation goals for kitchen shelf in map coordinates [x, y, theta]
+pre_kitchen: [-1.5, 0.3, 0]
+grasp: [-2.04, 0.3, 0]
+post_kitchen: [-1.5, 0.3, 0]
+deliver: [1, -0.5, -0.7]
+bla/blub: [1, 2]
+
diff --git a/cob_ehealth2012/launch/ehealth2012.launch b/cob_ehealth2012/launch/ehealth2012.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<launch>
+
+	<!-- start manipulation -->
+	<!-- <include file="$(find cob_manipulator)/ros/launch/kdl_solver.launch"/>  -->
+
+	<!-- upload default parameters -->
+	<include file="$(find cob_default_robot_config)/$(env ROBOT)/upload_param_$(env ROBOT).launch"/>
+	<include file="$(find cob_default_env_config)/upload_param.launch"/>
+
+	<!-- upload cob_experimentation_days parameters -->
+	<!-- <include file="$(find cob_experimentation_days)/config/upload_param.launch"/> -->
+	<!-- <include file="$(find cob_experimentation_days)/config/upload_param.launch"/> -->
+
+	<!-- start navigation -->
+        <include file="$(find cob_navigation_global)/launch/2dnav_ros_dwa.launch"/> 
+        <include file="$(find cob_navigation_global)/launch/2dnav_linear.xml"/>   
+
+
+	<!-- send ROBOT parameters to parameter server -->
+        <rosparam command="load" ns="/script_server/base" file="$(find ehealth2012)/config/$(env ROBOT_ENV)/navigation_goals.yaml"/>
+	<!--rosparam command="load" ns="/script_server/arm" file="$(find ehealth2012)/config/$(env ROBOT)/arm_joint_configurations.yaml"/-->
+</launch>
diff --git a/cob_ehealth2012/launch/ehealth2012_sim.launch b/cob_ehealth2012/launch/ehealth2012_sim.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<launch>
+
+	<!-- start manipulation -->
+	<!-- <include file="$(find cob_manipulator)/ros/launch/kdl_solver.launch"/>  -->
+
+	<!-- upload default parameters -->
+	<include file="$(find cob_default_robot_config)/$(env ROBOT)/upload_param_$(env ROBOT).launch"/>
+	<include file="$(find cob_default_env_config)/upload_param.launch"/>
+	<include file="$(find cob_gazebo_objects)/launch/upload_param.launch"/>
+
+	<!-- upload cob_experimentation_days parameters -->
+	<!-- <include file="$(find cob_experimentation_days)/config/upload_param.launch"/> -->
+	<!-- <include file="$(find cob_experimentation_days)/config/upload_param.launch"/> -->
+
+	<!-- start navigation -->
+        <include file="$(find cob_2dnav)/ros/launch/2dnav_dwa.launch"/> 
+        <include file="$(find cob_linear_nav)/ros/launch/linear_nav.launch"/>   
+
+
+	<!-- send ROBOT parameters to parameter server -->
+        <rosparam command="load" ns="/script_server/base" file="$(find ehealth2012)/config/navigation_goals.yaml"/>
+</launch>
diff --git a/cob_ehealth2012/mainpage.dox b/cob_ehealth2012/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ehealth2012 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.
+-->
+
+
+*/
diff --git a/cob_ehealth2012/manifest.xml b/cob_ehealth2012/manifest.xml
@@ -0,0 +1,30 @@
+<package>
+  <description brief="cob_ehealth2012">
+
+     cob_ehealth2012
+
+  </description>
+  <author>Tobias Sing</author>
+  <license>LGPL</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/cob_ehealth2012</url>
+  <depend package="cob_generic_states_experimental"/>
+  <depend package="cob_generic_states"/>
+  <depend package="rospy"/>
+  <depend package="smach"/>
+  <depend package="cob_script_server"/>
+  <depend package="cob_command_gui"/>
+  <depend package="cob_default_robot_config"/>
+  <depend package="cob_default_env_config"/>
+  <depend package="cob_dashboard"/>
+  <depend package="cob_bringup"/>
+  <depend package="cob_bringup_sim"/>
+  <python path="${prefix}/states"/>
+  <!-- <depend package="cob_manipulator"/>
+  <depend package="cob_2dnav"/>
+  <depend package="cob_linear_nav"/>
+  <depend package="cob_object_detection_fake"/> -->
+
+</package>
+
+
diff --git a/cob_ehealth2012/src/SMeHealth2012a.py b/cob_ehealth2012/src/SMeHealth2012a.py
@@ -0,0 +1,28 @@
+#!/usr/bin/python
+
+import roslib
+roslib.load_manifest('ehealth2012')
+import rospy
+import smach
+import smach_ros
+
+# auto-add these imports
+from PrepareRobot import *
+from SelectObjectFromKeyboard import *
+
+from simple_script_server import *  # import script
+
+sss = simple_script_server()
+
+class SMeHealth2012a(smach.StateMachine):
+    def __init__(self):
+        smach.StateMachine.__init__(self, outcomes=['ended'])
+
+        with self:
+            smach.StateMachine.add('PREPAREROBOT', PrepareRobot(),
+                                   transitions={'succeeded':'SELECTOBJECTFROMKEYBOARD'}) 
+            smach.StateMachine.add('SELECTOBJECTFROMKEYBOARD', SelectObjectFromKeyboard(),
+                                   transitions={'objectSelected':'ended',
+                                                'quit':'ended'})
+
+
diff --git a/cob_ehealth2012/src/SMeHealth2012a.pyc b/cob_ehealth2012/src/SMeHealth2012a.pyc
diff --git a/cob_ehealth2012/src/SMeHealth2012b.py b/cob_ehealth2012/src/SMeHealth2012b.py
@@ -0,0 +1,51 @@
+#!/usr/bin/python
+import roslib
+roslib.load_manifest('cob_ehealth2012')   # todo: additional command line parameter
+import rospy
+import smach
+import smach_ros
+
+from generic_navigation_states import *
+from generic_perception_states import *
+
+from simple_script_server import *  # import script
+
+from PrepareRobot import *
+from SelectObjectFromKeyboard import *
+from Grasp import *
+
+class SMeHealth2012b(smach.StateMachine):
+    def __init__(self):
+        smach.StateMachine.__init__(self, outcomes=['ended'])
+        with self:
+            smach.StateMachine.add('PREPAREROBOT', PrepareRobot(),
+                                   transitions={
+                                    'succeeded':'SELECTOBJECTFROMKEYBOARD'
+                                   })
+            smach.StateMachine.add('SELECTOBJECTFROMKEYBOARD', SelectObjectFromKeyboard(),
+                                   transitions={
+                                    'objectSelected':'MOVE_TO_PREGRASP_POSITION',
+                                    'quit':'ended'
+                                  })
+            smach.StateMachine.add('MOVE_TO_PREGRASP_POSITION', approach_pose([1,-0.5,3.14],mode="omni"),   
+                        transitions={'succeeded':'MOVE_TO_GRASP_POSITION', 
+                                                'failed':'ended'})
+
+            smach.StateMachine.add('MOVE_TO_GRASP_POSITION', approach_pose([1.5,-0.5,3.14],mode="linear"),
+                        transitions={'succeeded':'DETECT_OBJECT', 
+                                                'failed':'ended'})
+
+            smach.StateMachine.add('DETECT_OBJECT', detect_object(max_retries = 5),
+                        transitions={'succeeded':'GRASP', 
+                                                'no_object':'DETECT_OBJECT',    #no_object -> retry DETECT_OBJECT
+                                                'no_more_retries':'ended', # object_not_picked
+                                                'failed':'ended'})  # failed
+
+            smach.StateMachine.add('GRASP', Grasp(),
+                                   transitions={
+                                    'grasped':'ended',
+                                    'not_grasped':'ended', 
+                                    'failed':'ended'
+                                   })
+
+