fetch and carry on cob3-3

Author: ipa-fmw

cob_bringup/ros/launch/cob3-3.launch

@@ -21,7 +21,7 @@
 	<include file="$(find cob_hokuyo)/ros/launch/hokuyo.launch" />
 
 	<!-- upper robot -->
-	<!--include file="$(find cob_lbr)/launch/lbr.launch" /-->
+	<include file="$(find cob_lbr)/launch/lbr.launch" />
 	<include file="$(find cob_tray)/ros/launch/tray.launch" />
 	<include file="$(find cob_torso)/ros/launch/torso.launch" />
 	<include file="$(find cob_sdh)/ros/launch/sdh.launch" />
@@ -31,11 +31,12 @@
 	<!-- start additional packages -->
 	<include file="$(find cob_joint_state_aggregator)/ros/launch/joint_state_aggregator.launch" />
 	<include file="$(find cob_teleop)/ros/launch/teleop.launch" />
+	<include file="$(find cob_sound)/ros/launch/sound.launch" />
 
-
+<!--
 	<group>
 		<machine name="pc2" address="cob3-3-pc2" default="true"/>
-		<!--include file="$(find cob_camera_sensors)/ros/launch/all_cameras.launch" /-->
+		<include file="$(find cob_camera_sensors)/ros/launch/all_cameras.launch" />
 	</group>
-
+-->
 </launch>

cob_generic_states/src/generic_states.py

@@ -112,9 +112,9 @@ def execute(self, userdata):
 		# initialize components #
 		#########################
 
-		#handle_head = sss.init("head")
-		#if handle_head.get_error_code() != 0:
-		#	return 'failed'
+		handle_head = sss.init("head")
+		if handle_head.get_error_code() != 0:
+			return 'failed'
 
 		handle_torso = sss.init("torso")
 		if handle_torso.get_error_code() != 0:
@@ -140,9 +140,9 @@ def execute(self, userdata):
 		# recover components #
 		######################
 
-		#handle_head = sss.recover("head")
-		#if handle_head.get_error_code() != 0:
-		#	return 'failed'		
+		handle_head = sss.recover("head")
+		if handle_head.get_error_code() != 0:
+			return 'failed'		
 
 		handle_torso = sss.recover("torso")
 		if handle_torso.get_error_code() != 0:
@@ -152,7 +152,7 @@ def execute(self, userdata):
 		if handle_tray.get_error_code() != 0:
 			return 'failed'
 
-		#handle_arm = sss.recover("arm")
+		handle_arm = sss.recover("arm")
 		#if handle_arm.get_error_code() != 0:
 		#	return 'failed'
 
@@ -209,9 +209,10 @@ def execute(self, userdata):
 
 #------------------------------------------------------------------------------------------#
 
+# This state moves the robot to the given pose.
 class approach_pose(smach.State):
 
-	def __init__(self, pose = ""):
+	def __init__(self, pose = "", mode = "omni", move_second = "False"):
 
 		smach.State.__init__(
 			self,
@@ -220,8 +221,8 @@ def __init__(self, pose = ""):
 			output_keys=['pose', 'message'])
 
 		self.pose = pose
-
-		# This state moves the robot to the given pose.
+		self.mode = mode
+		self.move_second = move_second
 
 	def execute(self, userdata):
 
@@ -244,7 +245,7 @@ def execute(self, userdata):
 
 		# try reaching pose
 		handle_base = sss.move("base", pose, False)
-		move_second = False
+		move_second = self.move_second
 
 		timeout = 0
 		while True:
@@ -253,9 +254,6 @@ def execute(self, userdata):
 				handle_base = sss.move("base", pose, False)
 				move_second = True
 			elif (handle_base.get_state() == 3) and (move_second):
-				userdata.message = []
-				userdata.message.append(3)
-				userdata.message.append("Pose was succesfully reached")
 				return 'succeeded'			
 
 			# check if service is available

cob_script_server/src/simple_script_server.py

@@ -493,7 +493,8 @@ def move_traj(self,component_name,parameter_name,blocking):
 			rospy.logdebug("%s action server ready",action_server_name)
 
 		# set operation mode to position
-		self.set_operation_mode(component_name,"position")
+		if not component_name == "arm":
+			self.set_operation_mode(component_name,"position")
 
 		# sending goal
 		client_goal = JointTrajectoryGoal()