Merge pull request #330 from ipa-nhg/new_teleop_node

New teleop node

cob_bringup/robots/cob3-2.xml

@@ -107,7 +107,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 		<include file="$(find cob_bringup)/drivers/openni2.launch" >
 			<arg name="robot" value="$(arg robot)" />

cob_bringup/robots/cob3-6.xml

@@ -98,7 +98,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 
 		<!-- aggregated robot_state_publisher -->

cob_bringup/robots/cob3-9.xml

@@ -103,7 +103,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 
 		<!-- aggregated robot_state_publisher -->

cob_bringup/robots/cob4-2.xml

@@ -63,7 +63,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1"/>
 		</include>
 		<!--include file="$(find cob_bringup)/tools/base_collision_observer.launch" >
 			<arg name="robot" value="$(arg robot)" />

cob_bringup/robots/cob4-4.xml

@@ -55,7 +55,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v2"/>
 		</include>
 		<include file="$(find cob_script_server)/launch/script_server.launch" />
 		<!--include file="$(find cob_bringup)/tools/base_collision_observer.launch" >

cob_bringup/robots/cob4-6.xml

@@ -55,7 +55,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v2"/>
 		</include>
 		<include file="$(find cob_script_server)/launch/script_server.launch" />
 		<!--include file="$(find cob_bringup)/tools/base_collision_observer.launch" >

cob_bringup/robots/raw3-1.xml

@@ -82,7 +82,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 
 		<!-- aggregated robot_state_publisher -->

cob_bringup/robots/raw3-2.xml

@@ -51,7 +51,6 @@
 		<!-- start additional packages -->
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 		<include file="$(find cob_bringup)/tools/base_collision_observer.launch" >
 			<arg name="robot" value="$(arg robot)" />

cob_bringup/robots/raw3-3.xml

@@ -59,7 +59,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 		<include file="$(find cob_bringup)/tools/base_collision_observer.launch" >
 			<arg name="robot" value="$(arg robot)" />

cob_bringup/robots/raw3-4.xml

@@ -61,7 +61,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 		<include file="$(find cob_bringup)/tools/base_collision_observer.launch" >
 			<arg name="robot" value="$(arg robot)" />

cob_bringup/robots/raw3-5.xml

@@ -47,7 +47,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 		<include file="$(find cob_bringup)/tools/base_collision_observer.launch" >
 			<arg name="robot" value="$(arg robot)" />

cob_bringup/robots/raw3-6.xml

@@ -53,7 +53,6 @@
 		</include>
 		<include file="$(find cob_bringup)/tools/teleop.launch" >
 			<arg name="robot" value="$(arg robot)" />
-			<arg name="version" value="v1" />
 		</include>
 		<include file="$(find cob_bringup)/tools/base_collision_observer.launch" >
 			<arg name="robot" value="$(arg robot)" />

cob_bringup/tools/teleop.launch

@@ -3,12 +3,14 @@
 
 	<arg name="robot" default="$(optenv ROBOT !!NO_ROBOT_SET!!)"/>
 	<arg name="pkg_hardware_config" default="$(find cob_hardware_config)"/>
-	<arg name="version" default="v2"/>
 
-	<include file="$(find cob_bringup)/tools/teleop_$(arg version).xml">
+	<include file="$(find cob_bringup)/drivers/joy.launch">
 		<arg name="robot" value="$(arg robot)" />
 		<arg name="pkg_hardware_config" value="$(arg pkg_hardware_config)" />
 	</include>
 
-</launch>
+	<node ns="teleop" pkg="cob_teleop" type="cob_teleop" name="$(anon teleop_node)" cwd="node" respawn="false" output="screen">
+		<rosparam file="$(arg pkg_hardware_config)/$(arg robot)/config/teleop.yaml" command="load"/>
+	</node>
 
+</launch>

cob_hardware_config/cob3-2/config/teleop.yaml

@@ -1,49 +1,62 @@
+#cob3-2
+##########
 # common params
 run_factor: 2.0
+joy_num_buttons: 12
+joy_num_axes: 6
+joy_num_modes: 4
+mode_switch_button: 8
+light_action_name: /light/set_light
+
+# axes
+axis_vx: 1
+axis_vy: 0
+axis_vz: 5
+axis_roll: 4
+axis_pitch: 3
+axis_yaw: 2
 
 # buttons
-lower_neck_button: 6
-upper_neck_button: 4
-tray_button: 3
-arm_joint12_button: 0
-arm_joint34_button: 1
-arm_joint56_button: 2
-arm_joint7_button: 3
-base_safety_button: 4
 deadman_button: 5
+safety_button: 4
+init_button: 9
+
+#mode1: Base
 run_button: 7
 
-# axes
-axis_vx: 1
-axis_vy: 0
-axis_vth: 2
-up_down: 5 #tray--up/down; tilt--front/back, here we just name up_down
-left_right: 4 #pan--left/right
+#mode2: Trajectory controller
+torso_sss_default_target_button: 1
+head_sss_default_target_button: 3
+arm_sss_default_target_button: 2
+
+#mode3: Velocity group controller
+arm_joint1_button: 0
+arm_joint2_button: 1
+arm_joint3_button: 2
+arm_joint4_button: 3
 
-# component configuration
-modules: {
+up_down_button: 5
+right_left_button: 4
+
+components: {
   base: {
-    max_velocity: [0.3, 0.2, 0.3],
-    max_acceleration: [0.5, 0.5, 0.7]
-  },
-  tray: {
-    joint_names: ['tray_joint'],
-    joint_step: [0.1]
+    twist_topic_name: '/base_controller/command',
+    twist_max_velocity: [0.3, 0.2, 0.3],
+    twist_max_acc: [0.5, 0.5, 0.7]
   },
   torso: {
-    joint_names: ['torso_lower_neck_pan_joint','torso_lower_neck_tilt_joint','torso_upper_neck_pan_joint','torso_upper_neck_tilt_joint'],
-    joint_step: [0.05,0.05,0.075,0.075]
+    sss_default_target: "home",
+    velocity_topic_name: "/torso/joint_group_velocity_controller/command",
+    joint_velocity: [0.05, 0.05, 0.05, 0.05]
   },
   head: {
-    joint_names: ['head_axis_joint'],
-    joint_step: [0.0]
+    sss_default_target: "front",
+    velocity_topic_name: "/head/joint_group_velocity_controller/command",
+    joint_velocity: [0.05]
   },
   arm: {
-    joint_names: ['arm_1_joint','arm_2_joint','arm_3_joint','arm_4_joint','arm_5_joint','arm_6_joint','arm_7_joint'],
-    joint_step: [0.1,0.1,0.1,0.1,0.1,0.1,0.1]
-  },
-  gripper: {
-    joint_names: ['sdh_finger_11_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_21_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint'],
-    joint_step: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
+    sss_default_target: "home",
+    velocity_topic_name: "/arm/joint_group_velocity_controller/command",
+    joint_velocity: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
   }
 }

cob_hardware_config/cob3-6/config/teleop.yaml

@@ -1,49 +1,62 @@
+#cob3-6
+##########
 # common params
 run_factor: 2.0
+joy_num_buttons: 12
+joy_num_axes: 6
+joy_num_modes: 4
+mode_switch_button: 8
+light_action_name: /light/set_light
+
+# axes
+axis_vx: 1
+axis_vy: 0
+axis_vz: 5
+axis_roll: 4
+axis_pitch: 3
+axis_yaw: 2
 
 # buttons
-lower_neck_button: 6
-upper_neck_button: 4
-tray_button: 3
-arm_joint12_button: 0
-arm_joint34_button: 1
-arm_joint56_button: 2
-arm_joint7_button: 3
-base_safety_button: 4
 deadman_button: 5
+safety_button: 4
+init_button: 9
+
+#mode1: Base
 run_button: 7
 
-# axes
-axis_vx: 1
-axis_vy: 0
-axis_vth: 2
-up_down: 5 #tray--up/down; tilt--front/back, here we just name up_down
-left_right: 4 #pan--left/right
+#mode2: Trajectory controller
+torso_sss_default_target_button: 1
+head_sss_default_target_button: 3
+arm_sss_default_target_button: 2
+
+#mode3: Velocity group controller
+arm_joint1_button: 0
+arm_joint2_button: 1
+arm_joint3_button: 2
+arm_joint4_button: 3
 
-# component configuration
-modules: {
+up_down_button: 5
+right_left_button: 4
+
+components: {
   base: {
-    max_velocity: [0.3, 0.2, 0.3],
-    max_acceleration: [0.5, 0.5, 0.7]
-  },
-  tray: {
-    joint_names: ['tray_joint'],
-    joint_step: [0.1]
+    twist_topic_name: '/base_controller/command',
+    twist_max_velocity: [0.3, 0.2, 0.3],
+    twist_max_acc: [0.5, 0.5, 0.7]
   },
   torso: {
-    joint_names: ['torso_lower_neck_tilt_joint','torso_pan_joint','torso_upper_neck_tilt_joint'],
-    joint_step: [0.05,0.075,0.075]
+    sss_default_target: "home",
+    velocity_topic_name: "/torso/joint_group_velocity_controller/command",
+    joint_velocity: [0.05, 0.05, 0.05]
   },
   head: {
-    joint_names: ['head_axis_joint'],
-    joint_step: [0.0]
+    sss_default_target: "front",
+    velocity_topic_name: "/head/joint_group_velocity_controller/command",
+    joint_velocity: [0.05]
   },
   arm: {
-    joint_names: ['arm_1_joint','arm_2_joint','arm_3_joint','arm_4_joint','arm_5_joint','arm_6_joint','arm_7_joint'],
-    joint_step: [0.1,0.1,0.1,0.1,0.1,0.1,0.1]
-  },
-  gripper: {
-    joint_names: ['sdh_finger_11_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_21_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint'],
-    joint_step: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
+    sss_default_target: "home",
+    velocity_topic_name: "/arm/joint_group_velocity_controller/command",
+    joint_velocity: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
   }
 }