Merge pull request #593 from ipa-fmw/feature/head_and_sensorring

Feature/head and sensorring
ipa320 · Jan 20, 2017 · 47f837e · 47f837e
2 parents 8a9937f + 2604cae
commit 47f837e
Show file tree

Hide file tree

Showing 17 changed files with 102 additions and 137 deletions.
diff --git a/cob_bringup/components/canopen_base.launch b/cob_bringup/components/canopen_base.launch
@@ -1,15 +1,23 @@
 <?xml version="1.0"?>
 <launch>
 	<arg name="robot"/>
-	<arg name="component_name" default="base"/>
+	<arg name="component_name" value="base"/>
 	<arg name="pkg_hardware_config" default="$(find cob_hardware_config)"/>
+
 	<arg name="can_device" default="can0"/>
+	<arg name="interval_ms" value="20"/>
+	<arg name="use_external_sync" value="false"/>
+	<arg name="start_external_sync" value="false"/>
+
 	<arg name="sim" default="false"/>
 
 	<include unless="$(arg sim)" file="$(find cob_bringup)/drivers/canopen_402.launch">
 		<arg name="robot" value="$(arg robot)"/>
 		<arg name="component_name" value="$(arg component_name)"/>
 		<arg name="can_device" value="$(arg can_device)"/>
+		<arg name="interval_ms" value="$(arg interval_ms)"/>
+		<arg name="use_external_sync" value="$(arg use_external_sync)"/>
+		<arg name="start_external_sync" value="$(arg start_external_sync)"/>
 	</include>
 	<include file="$(find cob_bringup)/controllers/base_controller_plugin.launch">
 		<arg name="robot" value="$(arg robot)"/>

diff --git a/cob_bringup/components/canopen_generic.launch b/cob_bringup/components/canopen_generic.launch
@@ -2,14 +2,22 @@
 <launch>
 	<arg name="robot"/>
 	<arg name="component_name"/>
+
 	<arg name="can_device"/>
+	<arg name="interval_ms" default="10"/>
+	<arg name="use_external_sync" default="false"/>
+	<arg name="start_external_sync" default="$(arg use_external_sync)"/>
+
 	<arg name="cartesian_control" default="false"/>
 	<arg name="sim" default="false"/>
 
 	<include unless="$(arg sim)" file="$(find cob_bringup)/drivers/canopen_402.launch">
 		<arg name="robot" value="$(arg robot)"/>
 		<arg name="component_name" value="$(arg component_name)"/>
 		<arg name="can_device" value="$(arg can_device)"/>
+		<arg name="interval_ms" value="$(arg interval_ms)"/>
+		<arg name="use_external_sync" value="$(arg use_external_sync)"/>
+		<arg name="start_external_sync" value="$(arg start_external_sync)"/>
 	</include>
 	<include file="$(find cob_bringup)/controllers/generic_controller.launch">
 		<arg name="robot" value="$(arg robot)"/>

diff --git a/cob_bringup/drivers/canopen_402.launch b/cob_bringup/drivers/canopen_402.launch
@@ -5,11 +5,22 @@
 	<arg name="pkg_hardware_config" default="$(find cob_hardware_config)"/>
 	<arg name="component_name"/>
 	<arg name="can_device"/>
+	<arg name="interval_ms"/>
+	<arg name="overflow" default="0"/>
+	<arg name="use_external_sync"/>
+	<arg name="start_external_sync"/>
+
+	<!-- start external sync node if requested -->
+	<node ns="$(arg component_name)" pkg="canopen_chain_node" type="canopen_sync_node" name="sync_$(arg can_device)" output="screen" if="$(arg start_external_sync)">
+		<rosparam subst_value="True">{bus: { device: $(arg can_device) }, sync: { interval_ms: $(arg interval_ms), overflow: $(arg overflow) } }</rosparam>
+	</node>
 
-	<!-- parameters for CANopen node -->
+	<!-- start canopen node -->
 	<node ns="$(arg component_name)" pkg="canopen_motor_node" type="canopen_motor_node" name="driver" output="screen" clear_params="true">
-		<rosparam param="bus/device" subst_value="True">$(arg can_device)</rosparam>
 		<rosparam command="load" file="$(arg pkg_hardware_config)/$(arg robot)/config/$(arg component_name)_driver.yaml"/>
+		<rosparam subst_value="True">{bus: { device: $(arg can_device) } }</rosparam>
+		<rosparam subst_value="True" if="$(arg use_external_sync)">{sync: { interval_ms: $(arg interval_ms), overflow: $(arg overflow) } }</rosparam>
+		<param name="bus/master_allocator" value="canopen::ExternalMaster::Allocator" if="$(arg use_external_sync)"/>
 	</node>
 
 </launch>
diff --git a/cob_bringup/package.xml b/cob_bringup/package.xml
@@ -16,6 +16,7 @@
   <depend>roslaunch</depend>
 
   <exec_depend>camera1394</exec_depend>
+  <exec_depend>canopen_chain_node</exec_depend>
   <exec_depend>canopen_motor_node</exec_depend>
   <exec_depend>cob_android_script_server</exec_depend>
   <exec_depend>cob_base_drive_chain</exec_depend>

diff --git a/cob_bringup/robots/cob4-10.xml b/cob_bringup/robots/cob4-10.xml
@@ -116,21 +116,22 @@
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="torso"/>
 			<arg name="can_device" value="can0"/>
-			<arg name="cartesian_control" value="true"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/canopen_generic.launch">
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="sensorring"/>
 			<arg name="can_device" value="can1"/>
-			<arg name="cartesian_control" value="false"/>
+			<arg name="use_external_sync" value="true"/>
+			<arg name="start_external_sync" value="true"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/canopen_generic.launch">
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="head"/>
 			<arg name="can_device" value="can1"/>
-			<arg name="cartesian_control" value="false"/>
+			<arg name="use_external_sync" value="true"/>
+			<arg name="start_external_sync" value="false"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/cam3d_openni2.launch">

diff --git a/cob_bringup/robots/cob4-7.xml b/cob_bringup/robots/cob4-7.xml
@@ -116,35 +116,34 @@
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="torso"/>
 			<arg name="can_device" value="can0"/>
-			<arg name="cartesian_control" value="true"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/canopen_generic.launch">
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="arm_left"/>
 			<arg name="can_device" value="can3"/>
-			<arg name="cartesian_control" value="true"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/canopen_generic.launch">
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="arm_right"/>
 			<arg name="can_device" value="can2"/>
-			<arg name="cartesian_control" value="true"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/canopen_generic.launch">
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="sensorring"/>
 			<arg name="can_device" value="can1"/>
-			<arg name="cartesian_control" value="false"/>
+			<arg name="use_external_sync" value="true"/>
+			<arg name="start_external_sync" value="true"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/canopen_generic.launch">
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="component_name" value="head"/>
 			<arg name="can_device" value="can1"/>
-			<arg name="cartesian_control" value="true"/>
+			<arg name="use_external_sync" value="true"/>
+			<arg name="start_external_sync" value="false"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<include file="$(find cob_bringup)/components/cam3d_openni2.launch">

diff --git a/cob_default_robot_config/cob4-7/head_joint_configurations.yaml b/cob_default_robot_config/cob4-7/head_joint_configurations.yaml
@@ -1,4 +1,4 @@
-joint_names: ["head_2_joint","head_3_joint"]
+joint_names: ["head_1_joint", "head_2_joint", "head_3_joint"]
 action_name: /head/joint_trajectory_controller/follow_joint_trajectory
 service_ns: /head/driver
 default_vel: 0.6
@@ -17,8 +17,8 @@ back_up_full: [[1.57, -3.14, 4.71]]
 back_down: [[-0.5, 1.0, 2.64]]
 back_down_full: [[-1.57, 3.14, 1.57]]
 
-left: [[1.57, 0.0, 0.0]]
-right: [[-1.57, 0.0, 0.0]]
+left: [[0.0, 0.0, 1.57]]
+right: [[0.0, 0.0, -1.57]]
 
 nod: [front_down, front]
 shake: [left, right, left, home]
diff --git a/cob_hardware_config/cob4-1/config/diagnostics_analyzers.yaml b/cob_hardware_config/cob4-1/config/diagnostics_analyzers.yaml
@@ -104,27 +104,19 @@ analyzers:
       torso:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Torso
-        contains: 'torso/driver'
+        contains: 'torso/'
 #      head:
 #        type: diagnostic_aggregator/GenericAnalyzer
 #        path: Head
-#        contains: 'head/driver'
+#        contains: 'head/'
 #      sensorring:
 #        type: diagnostic_aggregator/GenericAnalyzer
 #        path: Sensorring
-#        contains: 'sensorring/driver'
+#        contains: 'sensorring/'
       base:
-        type: diagnostic_aggregator/AnalyzerGroup
+        type: diagnostic_aggregator/GenericAnalyzer
         path: Base
-        analyzers:
-          base:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            contains: 'base/driver'
-          base_twist_mux:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: twist_mux
-            contains: 'twist_mux'
+        contains: 'base/'
 
   safety:
     type: diagnostic_aggregator/GenericAnalyzer

diff --git a/cob_hardware_config/cob4-10/config/diagnostics_analyzers.yaml b/cob_hardware_config/cob4-10/config/diagnostics_analyzers.yaml
@@ -104,27 +104,19 @@ analyzers:
       torso:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Torso
-        contains: 'torso/driver'
+        contains: 'torso/'
       head:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Head
-        contains: 'head/driver'
+        contains: 'head/'
       sensorring:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Sensorring
-        contains: 'sensorring/driver'
+        contains: 'sensorring/'
       base:
-        type: diagnostic_aggregator/AnalyzerGroup
+        type: diagnostic_aggregator/GenericAnalyzer
         path: Base
-        analyzers:
-          base:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            contains: 'base/driver'
-          base_twist_mux:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: twist_mux
-            contains: 'twist_mux'
+        contains: 'base/'
 
   safety:
     type: diagnostic_aggregator/GenericAnalyzer

diff --git a/cob_hardware_config/cob4-2/config/diagnostics_analyzers.yaml b/cob_hardware_config/cob4-2/config/diagnostics_analyzers.yaml
@@ -104,35 +104,27 @@ analyzers:
       torso:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Torso
-        contains: 'torso/driver'
+        contains: 'torso/'
       arm_right:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Arm Right
-        contains: 'arm_right/driver'
+        contains: 'arm_right/'
       arm_left:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Arm Left
-        contains: 'arm_left/driver'
+        contains: 'arm_left/'
 #      sensorring:
 #        type: diagnostic_aggregator/GenericAnalyzer
 #        path: Sensorring
-#        contains: 'sensorring/driver'
+#        contains: 'sensorring/'
 #      head:
 #        type: diagnostic_aggregator/GenericAnalyzer
 #        path: Head
-#        contains: 'head/driver'
+#        contains: 'head/'
       base:
-        type: diagnostic_aggregator/AnalyzerGroup
+        type: diagnostic_aggregator/GenericAnalyzer
         path: Base
-        analyzers:
-          base:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            contains: 'base/driver'
-          base_twist_mux:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: twist_mux
-            contains: 'twist_mux'
+        contains: 'base/'
 
   safety:
     type: diagnostic_aggregator/GenericAnalyzer

diff --git a/cob_hardware_config/cob4-3/config/diagnostics_analyzers.yaml b/cob_hardware_config/cob4-3/config/diagnostics_analyzers.yaml
@@ -24,17 +24,9 @@ analyzers:
     path: Actuators
     analyzers:
       base:
-        type: diagnostic_aggregator/AnalyzerGroup
+        type: diagnostic_aggregator/GenericAnalyzer
         path: Base
-        analyzers:
-          base:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            startswith: 'base/driver'
-          base_twist_mux:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: twist_mux
-            contains: 'twist_mux'
+        contains: 'base/'
 
   sensors:
     type: diagnostic_aggregator/AnalyzerGroup

diff --git a/cob_hardware_config/cob4-4/config/diagnostics_analyzers.yaml b/cob_hardware_config/cob4-4/config/diagnostics_analyzers.yaml
@@ -24,17 +24,9 @@ analyzers:
     path: Actuators
     analyzers:
       base:
-        type: diagnostic_aggregator/AnalyzerGroup
+        type: diagnostic_aggregator/GenericAnalyzer
         path: Base
-        analyzers:
-          base:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            startswith: 'base/driver'
-          base_twist_mux:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: twist_mux
-            contains: 'twist_mux'
+        contains: 'base/'
 
   sensors:
     type: diagnostic_aggregator/AnalyzerGroup

diff --git a/cob_hardware_config/cob4-5/config/diagnostics_analyzers.yaml b/cob_hardware_config/cob4-5/config/diagnostics_analyzers.yaml
@@ -81,26 +81,14 @@ analyzers:
     type: diagnostic_aggregator/AnalyzerGroup
     path: Actuators
     analyzers:
-      base:
-        type: diagnostic_aggregator/AnalyzerGroup
-        path: Base
-        analyzers:
-          base:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            contains: 'base/driver'
-          base_twist_mux:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: twist_mux
-            contains: 'twist_mux'
       arm_left:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Arm Left
-        contains: 'arm_left/driver'
+        contains: 'arm_left/'
       arm_right:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Arm Right
-        contains: 'arm_right/driver'
+        contains: 'arm_right/'
       gripper_left:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Gripper Left
@@ -112,7 +100,12 @@ analyzers:
       sensorring:
         type: diagnostic_aggregator/GenericAnalyzer
         path: Sensorring
-        contains: 'sensorring/driver'
+        contains: 'sensorring/'
+      base:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: Base
+        contains: 'base/'
+
   safety:
     type: diagnostic_aggregator/GenericAnalyzer
     path: Safety

diff --git a/cob_hardware_config/cob4-6/config/diagnostics_analyzers.yaml b/cob_hardware_config/cob4-6/config/diagnostics_analyzers.yaml
@@ -24,17 +24,9 @@ analyzers:
     path: Actuators
     analyzers:
       base:
-        type: diagnostic_aggregator/AnalyzerGroup
+        type: diagnostic_aggregator/GenericAnalyzer
         path: Base
-        analyzers:
-          base:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            startswith: 'base/driver'
-          base_twist_mux:
-            type: diagnostic_aggregator/GenericAnalyzer
-            path: twist_mux
-            contains: 'twist_mux'
+        contains: 'base/'
 
   sensors:
     type: diagnostic_aggregator/AnalyzerGroup