Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP

Comparing changes

Choose two branches to see what's changed or to start a new pull request. If you need to, you can also compare across forks.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also compare across forks.
...
  • 7 commits
  • 22 files changed
  • 0 commit comments
  • 2 contributors
Showing with 136 additions and 74 deletions.
  1. +3 −0  cob_ehealth2012/CMakeLists.txt
  2. +24 −7 cob_ehealth2012/launch/ehealth2012.launch
  3. +6 −18 cob_ehealth2012/launch/ehealth2012_sim.launch
  4. +6 −6 cob_ehealth2012/manifest.xml
  5. BIN  cob_ehealth2012/src/SMeHealth2012a.pyc
  6. +29 −15 cob_ehealth2012/src/SMeHealth2012b.py
  7. BIN  cob_ehealth2012/src/SMeHealth2012b.pyc
  8. +1 −4 cob_experimentation_days/CMakeLists.txt
  9. +1 −1  cob_experimentation_days/launch/experimentation_days_robot.launch
  10. +1 −2  cob_experimentation_days/launch/experimentation_days_sim.launch
  11. +1 −2  cob_experimentation_days/manifest.xml
  12. +2 −2 cob_generic_states/manifest.xml
  13. +1 −0  cob_generic_states/src/generic_basic_states.py
  14. +2 −1  cob_generic_states/src/generic_perception_states.py
  15. +9 −6 cob_generic_states_experimental/src/Grasp.py
  16. BIN  cob_generic_states_experimental/src/Grasp.pyc
  17. +11 −9 cob_generic_states_experimental/src/PrepareRobot.py
  18. BIN  cob_generic_states_experimental/src/PrepareRobot.pyc
  19. +37 −0 cob_generic_states_experimental/src/PutObjectOnTray.py
  20. +1 −0  cob_generic_states_experimental/src/SelectObjectFromKeyboard.py
  21. BIN  cob_generic_states_experimental/src/SelectObjectFromKeyboard.pyc
  22. +1 −1  stack.xml
View
3  cob_ehealth2012/CMakeLists.txt
@@ -28,3 +28,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
+
+# rostest
+rosbuild_add_roslaunch_check(launch ROBOT=cob3-3)
View
31 cob_ehealth2012/launch/ehealth2012.launch
@@ -1,23 +1,40 @@
<?xml version="1.0"?>
<launch>
+ <arg name="pc1" default="$(env ROBOT)-pc1"/>
+ <arg name="pc2" default="$(env ROBOT)-pc2"/>
+ <arg name="pc3" default="$(env ROBOT)-pc3"/>
+
<!-- start manipulation -->
- <!-- <include file="$(find cob_manipulator)/ros/launch/kdl_solver.launch"/> -->
+ <include file="$(find cob_manipulator)/ros/launch/kdl_solver.launch"/>
+
+ <include file="$(find cob_tray_sensors)/ros/launch/tray_sensors.launch"/>
<!-- upload default parameters -->
- <include file="$(find cob_default_robot_config)/$(env ROBOT)/upload_param_$(env ROBOT).launch"/>
+ <include file="$(find cob_default_robot_config)/upload_param.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"/> -->
+ <!-- send ROBOT parameters to parameter server -->
+ <rosparam command="load" ns="/script_server/base" file="$(find cob_ehealth2012)/config/$(env ROBOT_ENV)/navigation_goals.yaml"/>
+ <!--rosparam command="load" ns="/script_server/arm" file="$(find cob_ehealth2012)/config/$(env ROBOT)/arm_joint_configurations.yaml"/-->
+
+
<!-- start navigation -->
- <include file="$(find cob_navigation_global)/launch/2dnav_ros_dwa.launch"/>
- <include file="$(find cob_navigation_global)/launch/2dnav_linear.xml"/>
+ <include file="$(find cob_navigation_global)/launch/2dnav_ros_dwa.launch"/>
+ <include file="$(find cob_navigation_global)/launch/2dnav_linear.xml"/>
+
+ <group>
+ <machine name="pc3" address="$(arg pc3)" default="true"/>
+
+ <include file="$(find cob_sensor_fusion)/ros/launch/sensor_fusion_stereo.launch" />
+ <include file="$(find cob_object_detection)/ros/launch/object_detection.launch" />
+ </group>
+
+ <machine name="pc1" address="$(arg pc1)" default="true"/>
- <!-- 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>
View
24 cob_ehealth2012/launch/ehealth2012_sim.launch
@@ -1,23 +1,11 @@
<?xml version="1.0"?>
<launch>
- <!-- start manipulation -->
- <!-- <include file="$(find cob_manipulator)/ros/launch/kdl_solver.launch"/> -->
+ <!-- start ehealth components -->
+ <include file="$(find cob_ehealth2012)/launch/ehealth2012.launch">
+ <arg name="pc1" value="localhost"/>
+ <arg name="pc2" value="localhost"/>
+ <arg name="pc3" value="localhost"/>
+ </include>
- <!-- 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>
View
12 cob_ehealth2012/manifest.xml
@@ -19,12 +19,12 @@
<depend package="cob_dashboard"/>
<depend package="cob_bringup"/>
<depend package="cob_bringup_sim"/>
+ <depend package="cob_navigation_global"/>
+ <depend package="cob_manipulator"/>
+ <depend package="cob_object_detection"/>
+ <depend package="cob_sensor_fusion"/>
+ <depend package="cob_tray_sensors"/>
+
<python path="${prefix}/states"/>
- <!-- <depend package="cob_manipulator"/>
- <depend package="cob_2dnav"/>
- <depend package="cob_linear_nav"/>
- <depend package="cob_object_detection_fake"/> -->
</package>
-
-
View
BIN  cob_ehealth2012/src/SMeHealth2012a.pyc
Binary file not shown
View
44 cob_ehealth2012/src/SMeHealth2012b.py
@@ -5,6 +5,7 @@
import smach
import smach_ros
+from generic_basic_states import *
from generic_navigation_states import *
from generic_perception_states import *
@@ -13,39 +14,52 @@
from PrepareRobot import *
from SelectObjectFromKeyboard import *
from Grasp import *
+from PutObjectOnTray import *
class SMeHealth2012b(smach.StateMachine):
def __init__(self):
- smach.StateMachine.__init__(self, outcomes=['ended'])
+ smach.StateMachine.__init__(self, outcomes=['ended','failed'])
with self:
- smach.StateMachine.add('PREPAREROBOT', PrepareRobot(),
+ smach.StateMachine.add('PREPARE_ROBOT', PrepareRobot(),
transitions={
- 'succeeded':'SELECTOBJECTFROMKEYBOARD'
+ 'succeeded':'SELECT_OBJECT_FROM_KEYBOARD'
})
- smach.StateMachine.add('SELECTOBJECTFROMKEYBOARD', SelectObjectFromKeyboard(),
+ smach.StateMachine.add('SELECT_OBJECT_FROM_KEYBOARD', 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"),
+ smach.StateMachine.add('MOVE_TO_PREGRASP_POSITION', approach_pose([1,-0.5,0],mode="omni"),
transitions={'succeeded':'MOVE_TO_GRASP_POSITION',
- 'failed':'ended'})
+ 'failed':'failed'})
- smach.StateMachine.add('MOVE_TO_GRASP_POSITION', approach_pose([1.5,-0.5,3.14],mode="linear"),
+ smach.StateMachine.add('MOVE_TO_GRASP_POSITION', approach_pose([0,-0.5,0],mode="linear"),
transitions={'succeeded':'DETECT_OBJECT',
- 'failed':'ended'})
+ 'failed':'failed'})
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
+ 'no_more_retries':'SELECT_OBJECT_FROM_KEYBOARD', # object_not_picked
+ 'failed':'failed'}) # failed
smach.StateMachine.add('GRASP', Grasp(),
transitions={
- 'grasped':'ended',
+ 'grasped':'PUT_OBJECT_ON_TRAY',
'not_grasped':'ended',
- 'failed':'ended'
- })
-
-
+ 'failed':'failed'})
+
+ smach.StateMachine.add('PUT_OBJECT_ON_TRAY', PutObjectOnTray(),
+ transitions={
+ 'succeeded':'MOVE_TO_DELIVER_POSITION',
+ 'failed':'failed'})
+
+ smach.StateMachine.add('MOVE_TO_DELIVER_POSITION', approach_pose([2,-1,-1.57],mode="omni"),
+ transitions={'succeeded':'DELIVER',
+ 'failed':'failed'})
+
+ smach.StateMachine.add('DELIVER', deliver_object(),
+ transitions={
+ 'succeeded':'SELECT_OBJECT_FROM_KEYBOARD',
+ 'retry':'DELIVER',
+ 'failed':'failed'})
View
BIN  cob_ehealth2012/src/SMeHealth2012b.pyc
Binary file not shown
View
5 cob_experimentation_days/CMakeLists.txt
@@ -30,7 +30,4 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#target_link_libraries(example ${PROJECT_NAME})
# rostest
-rosbuild_add_roslaunch_check(launch/experimentation_days.launch ROBOT=cob3-3 ROBOT_ENV=ipa-kitchen)
-#rosbuild_add_roslaunch_check(launch/experimentation_days_robot.launch ROBOT=cob3-3 ROBOT_ENV=ipa-kitchen)
-rosbuild_add_roslaunch_check(launch/experimentation_days_sim.launch ROBOT=cob3-3 ROBOT_ENV=ipa-kitchen)
-rosbuild_add_roslaunch_check(launch/dashboard.launch ROBOT=cob3-3 ROBOT_ENV=ipa-kitchen)
+rosbuild_add_roslaunch_check(launch ROBOT=cob3-3 ROBOT_ENV=ipa-kitchen)
View
2  cob_experimentation_days/launch/experimentation_days_robot.launch
@@ -10,7 +10,7 @@
<include file="$(find cob_experimentation_days)/launch/experimentation_days.launch"/>
<!-- start navigation -->
- <include file="$(find cob_2dnav)/ros/launch/2dnav_ipa.launch"/>
+ <include file="$(find cob_navigation_global)/launch/2dnav_ros_dwa.launch"/>
<!-- start object detection -->
<machine name="pc2" address="cob3-3-pc2" default="true"/>
View
3  cob_experimentation_days/launch/experimentation_days_sim.launch
@@ -8,8 +8,7 @@
<include file="$(find cob_experimentation_days)/launch/experimentation_days.launch"/>
<!-- start navigation -->
- <include file="$(find cob_2dnav)/ros/launch/2dnav_dwa.launch"/>
- <include file="$(find cob_linear_nav)/ros/launch/linear_nav.launch"/>
+ <include file="$(find cob_navigation_global)/launch/2dnav_ros_dwa.launch"/>
<!-- start packages only needed for simulation-->
<include file="$(find cob_object_detection_fake)/ros/launch/object_detection_fake.launch"/>
View
3  cob_experimentation_days/manifest.xml
@@ -19,8 +19,7 @@
<depend package="cob_bringup"/>
<depend package="cob_bringup_sim"/>
<depend package="cob_manipulator"/>
- <depend package="cob_2dnav"/>
- <depend package="cob_linear_nav"/>
+ <depend package="cob_navigation_global"/>
<depend package="cob_object_detection_fake"/>
View
4 cob_generic_states/manifest.xml
@@ -22,7 +22,7 @@
<depend package="cob_object_detection_fake"/>
<!-- this dependencies should better be in a separate msgs package to reduce dependencies-->
- <!-- <depend package="cob_mmcontroller"/>
- <depend package="cob_tray_sensors"/> -->
+ <!-- <depend package="cob_mmcontroller"/-->
+ <depend package="cob_tray_sensors"/>
</package>
View
1  cob_generic_states/src/generic_basic_states.py
@@ -229,6 +229,7 @@ def __init__(self):
input_keys=['object_name'])
def execute(self, userdata):
+ sss.move("head","front",False)
sss.say(["Here is your " + userdata.object_name + ". Please help yourself."],False)
sss.move("torso","nod",False)
View
3  cob_generic_states/src/generic_perception_states.py
@@ -111,7 +111,7 @@ def execute(self, userdata):
self.retries = 0
handle_torso = sss.move("torso","home",False)
handle_torso.wait()
- handle_arm = sss.move("arm","look_at_table-to-folded",False)
+ handle_arm = sss.move("arm","look_at_table-to-folded")
return 'no_more_retries'
# move sdh as feedback
@@ -181,6 +181,7 @@ def execute(self, userdata):
return 'no_object'
# we succeeded to detect an object
+ sss.move("torso","home")
userdata.object = obj
self.retries = 0
return 'succeeded'
View
15 cob_generic_states_experimental/src/Grasp.py
@@ -7,9 +7,12 @@
sss = simple_script_server()
class Grasp(smach.State):
- def __init__(self):
- smach.State.__init__(self,
- outcomes=['grasped','not_grasped','failed'])
- input_keys=['object_name'])
- def execute(self, userdata):
- return 'grasped'
+ def __init__(self):
+ smach.State.__init__(self,
+ outcomes=['grasped','not_grasped','failed'],
+ input_keys=['object_name'])
+ def execute(self, userdata):
+ sss.say(["I am grasping " + userdata.object_name + " now."])
+ sss.sleep(2)
+ sss.say(["I grasped " + userdata.object_name + "."])
+ return 'grasped'
View
BIN  cob_generic_states_experimental/src/Grasp.pyc
Binary file not shown
View
20 cob_generic_states_experimental/src/PrepareRobot.py
@@ -12,19 +12,21 @@ def __init__(self):
outcomes=['succeeded'])
def execute(self, userdata):
sss.sleep(2) # 2
- sss.say(["Preparing."])
+ sss.say(["Preparing."],False)
# bring robot into the starting state
- #handle_tray = sss.move("tray","down",False)
- #handle_torso = sss.move("torso","home",False)
- #handle_arm = sss.move("arm","folded",False)
- #handle_sdh = sss.move("sdh","cylclosed",False)
+ handle_tray = sss.move("tray","down",False)
+ handle_torso = sss.move("torso","home",False)
+ handle_arm = sss.move("arm","folded",False)
+ handle_sdh = sss.move("sdh","cylclosed",False)
+ handle_head = sss.move("head","front",False)
# wait for all movements to be finished
- #handle_tray.wait()
- #handle_torso.wait()
- #handle_arm.wait()
- #handle_sdh.wait()
+ handle_tray.wait()
+ handle_torso.wait()
+ handle_arm.wait()
+ handle_sdh.wait()
+ handle_head.wait()
# announce ready
sss.say(["Ready."])
View
BIN  cob_generic_states_experimental/src/PrepareRobot.pyc
Binary file not shown
View
37 cob_generic_states_experimental/src/PutObjectOnTray.py
@@ -0,0 +1,37 @@
+import roslib
+roslib.load_manifest('cob_generic_states_experimental')
+import rospy
+import smach
+import smach_ros
+from simple_script_server import * # import script
+sss = simple_script_server()
+
+## Put object on tray side state
+#
+# This state puts a side grasped object on the tray
+class PutObjectOnTray(smach.State):
+
+ def __init__(self):
+ smach.State.__init__(
+ self,
+ outcomes=['succeeded', 'failed'])
+
+ def execute(self, userdata):
+ #TODO select position on tray depending on how many objects are on the tray already
+
+ # move object to frontside
+ handle_arm = sss.move("arm","grasp-to-tray",False)
+ sss.move("head","front",False)
+ sss.sleep(2)
+ sss.move("tray","up")
+ handle_arm.wait()
+
+ # release object
+ sss.move("sdh","cylopen")
+
+ # move arm to backside again
+ handle_arm = sss.move("arm","tray-to-folded",False)
+ sss.sleep(3)
+ sss.move("sdh","home")
+ handle_arm.wait()
+ return 'succeeded'
View
1  cob_generic_states_experimental/src/SelectObjectFromKeyboard.py
@@ -18,6 +18,7 @@ def execute(self, userdata):
print str(i) + ' ' + proto_objects[i]
objectID = ""
while objectID not in [str(i) for i in range(len(proto_objects))]:
+ #objectID = "1" #FIXME hardcoded to milk for testing
objectID = raw_input('Please select an object: ')
print 'You selected #' + objectID
objectID = int(objectID)
View
BIN  cob_generic_states_experimental/src/SelectObjectFromKeyboard.pyc
Binary file not shown
View
2  stack.xml
@@ -9,7 +9,7 @@
<depend stack="cob_driver" /> <!-- cob_tray_sensors -->
<depend stack="cob_environments" /> <!-- cob_default_env_config -->
<depend stack="cob_manipulation" /> <!-- cob_mmcontroller, cob_manipulator -->
- <depend stack="cob_navigation" /> <!-- cob_linear_nav, cob_2dnav -->
+ <depend stack="cob_navigation" /> <!-- cob_navigation_global -->
<depend stack="cob_object_perception" /> <!-- cob_object_detection_msgs, cob_object_detection_fake -->
<depend stack="cob_robots" /> <!-- cob_bringup, cob_default_robot_config -->
<depend stack="cob_simulation" /> <!-- cob_bringup_sim -->

No commit comments for this range

Something went wrong with that request. Please try again.