Merge pull request #30 from INTERACT-tud-amr/ft_multidingo

Ft multidingo
INTERACT-tud-amr · Jun 28, 2024 · 120e546 · 120e546
2 parents 01e5247 + b5d57d9
commit 120e546
Show file tree

Hide file tree

Showing 7 changed files with 190 additions and 107 deletions.
diff --git a/dinova_bringup/config/vicon_dingo1.yaml b/dinova_bringup/config/vicon_dingo1.yaml
@@ -1,11 +1,45 @@
 object_names:
   - "dingo1"
+  - "dingo2"
+  - "tablesmall_1"
+  - "tablesmall_2"
+  - "cup1"
+  - "cup2"
+  - "wineglass"
+  - "dynamic_object2"
 object_msg_types: # "geometry_msgs/{PoseStamped,TransformStamped,PoseWithCovarianceStamped}"
   - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
 object_frame_ids:
   - "map"
+  - "map"
+  - "map"
+  - "map"
+  - "map"
+  - "map"
+  - "map"
+  - "map"
 object_frequency_divider:
   - 2
+  - 2
+  - 2
+  - 2
+  - 2
+  - 2
+  - 2
+  - 2
 object_publish_topics:
   - "/vicon/dingo1"
-
+  - "/vicon/dingo2"
+  - "/vicon/tablesmall_1"
+  - "/vicon/tablesmall_2"
+  - "/vicon/cup1"
+  - "/vicon/cup2"
+  - "/vicon/wineglass"
+  - "/vicon/dynamic_object2"
diff --git a/dinova_bringup/config/vicon_dingo2.yaml b/dinova_bringup/config/vicon_dingo2.yaml
@@ -1,16 +1,45 @@
 object_names:
+  - "dingo1"
   - "dingo2"
-  - "mug1"
+  - "tablesmall_1"
+  - "tablesmall_2"
+  - "cup1"
+  - "cup2"
+  - "wineglass"
+  - "dynamic_object2"
 object_msg_types: # "geometry_msgs/{PoseStamped,TransformStamped,PoseWithCovarianceStamped}"
   - "geometry_msgs/PoseStamped"
   - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
+  - "geometry_msgs/PoseStamped"
 object_frame_ids:
   - "map"
   - "map"
+  - "map"
+  - "map"
+  - "map"
+  - "map"
+  - "map"
+  - "map"
 object_frequency_divider:
   - 2
   - 2
+  - 2
+  - 2
+  - 2
+  - 2
+  - 2
+  - 2
 object_publish_topics:
+  - "/vicon/dingo1"
   - "/vicon/dingo2"
-  - "/vicon/mug1"
-
+  - "/vicon/tablesmall_1"
+  - "/vicon/tablesmall_2"
+  - "/vicon/cup1"
+  - "/vicon/cup2"
+  - "/vicon/wineglass"
+  - "/vicon/dynamic_object2"
diff --git a/dinova_bringup/launch/dingo.launch b/dinova_bringup/launch/dingo.launch
@@ -3,80 +3,92 @@
     <!-- Dingo params: Dont change! -->
     <arg name="motor_params" default="$(optenv DINGO_MOTOR_PARAMS base)" />
     <arg name="config" default="$(optenv DINGO_CONFIG base)" />
+
     <!-- Velodyne lidar -->
     <arg name="lidar" default="false"/>
-    <arg name="velodyne_frame_id" default="velodyne_base_link"/>
 
      <!-- Vicon -->
     <arg name="vicon" default="true" />
     <arg name="vicon_config_file" default="$(find dinova_bringup)/config/vicon_$(env ROBOT_NAME).yaml"/>
-    <param name="/vicon/use_vicon" type="bool" value="$(arg vicon)" />
-    <param name="/vicon/dingo_topic" type="string" value="/vicon/$(env ROBOT_NAME)" />
-
-
-    <!-- Robot model -->
-    <param name="robot_description" command="$(find xacro)/xacro '$(find dinova_description)/urdf/real_dingo-o.urdf.xacro' 
+
+
+    <group ns="$(env ROBOT_NAME)">
+        <param name="vicon/use_vicon" type="bool" value="$(arg vicon)" />
+        <param name="vicon/dingo_topic" type="string" value="/vicon/$(env ROBOT_NAME)" />
+
+        <!-- Robot model -->
+        <param name="robot_description" command="$(find xacro)/xacro '$(find dinova_description)/urdf/real_dingo-o.urdf.xacro' 
                                                     lidar:=$(arg lidar)"/>
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
-
-    <!-- DINGO Nodes -->
-
-    <node pkg="rosserial_python" type="message_info_service.py" name="rosserial_message_info" />
-
-    <node pkg="dingo_base" type="dingo_node" name="dingo_node" output="screen">
-        <param name="dingo_omni" type="bool" value="$(optenv DINGO_OMNI 0)" />
-        <rosparam command="load" file="$(find dingo_base)/config/$(arg motor_params).yaml" />
-        <param name="wireless_interface" value="$(optenv DINGO_WIRELESS_INTERFACE wlp2s0)" />
-    </node>
-
-    <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter">
-        <rosparam file="$(eval optenv('DINGO_MAG_CONFIG', find('dingo_base')+'/config/mag_config_default.yaml'))" />
-        <rosparam>
-        gain: 0.1
-        zeta: 0.001
-        publish_tf: false
-        use_magnetic_field_msg: true
-        </rosparam>
-    </node>
+        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+            <param name="tf_prefix" value="$(env ROBOT_NAME)"/>
+        </node>
 
-    <!-- Differential controller and basic localization -->
-    <include file="$(find dingo_control)/launch/control.launch" />
+        <!-- DINGO Nodes -->
 
-    <!-- Teleoperation via joystick and rviz interactive markers -->
-    <include file="$(find dingo_control)/launch/teleop.launch" />
+        <node pkg="rosserial_python" type="message_info_service.py" name="rosserial_message_info" />
 
-    <!-- Diagnostic Aggregator for robot monitor usage -->
-    <group if="$(optenv DINGO_OMNI 0)">
-        <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
-        <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
-        <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_omni.yaml" />
+        <node pkg="dingo_base" type="dingo_node" name="dingo_node" output="screen">
+            <param name="dingo_omni" type="bool" value="$(optenv DINGO_OMNI 0)" />
+            <rosparam command="load" file="$(find dingo_base)/config/$(arg motor_params).yaml" />
+            <param name="wireless_interface" value="$(optenv DINGO_WIRELESS_INTERFACE wlp2s0)" />
+            <remap from="/imu/data_raw" to="imu/data_raw" />
+            <remap from="/diagnostics" to="diagnostics" />
         </node>
-    </group>
-    <group unless="$(optenv DINGO_OMNI 0)">
-        <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
-        <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
-        <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_diff.yaml" />
-        </node>
-    </group>
 
-    <!-- Battery checking -->
-    <node pkg="dinova_bringup" type="battery_checker" name="battery_checker"/>
+        <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter">
+            <rosparam file="$(eval optenv('DINGO_MAG_CONFIG', find('dingo_base')+'/config/mag_config_default.yaml'))" />
+            <rosparam>
+                gain: 0.1
+                zeta: 0.001
+                publish_tf: false
+                use_magnetic_field_msg: true
+            </rosparam>
+        </node>
 
-    <!-- Vicon Nodes -->
-    <group if="$(arg vicon)">
-        <rosparam ns="vicon/object_specific" command="load" file="$(arg vicon_config_file)"/>
-        <include file="$(find dinova_bringup)/launch/vicon.launch"/>
-        <node pkg="tf" type="static_transform_publisher" name="$(env ROBOT_NAME)_static_tf" args="0 0 0 0 0 0 $(env ROBOT_NAME) base_link 0.1" />
-    </group>
+        <!-- Differential controller and basic localization -->
+        <include file="$(find dinova_control)/launch/dingo_control.launch" />
+
+        <!-- Teleoperation via joystick and rviz interactive markers -->
+        <include file="$(find dinova_control)/launch/dingo_teleop.launch" />
+
+        <!-- Diagnostic Aggregator for robot monitor usage -->
+        <group if="$(optenv DINGO_OMNI 0)">
+            <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_omni.yaml" />
+                <remap from="/diagnostics" to="diagnostics" />
+                <remap from="/diagnostics_agg" to="diagnostics_agg" />
+                <remap from="/diagnostics_toplevel_state" to="diagnostics_toplevel_state" />
+            </node>
+        </group>
+        <group unless="$(optenv DINGO_OMNI 0)">
+            <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_diff.yaml" />
+                <remap from="/diagnostics" to="diagnostics" />
+                <remap from="/diagnostics_agg" to="diagnostics_agg" />
+                <remap from="/diagnostics_toplevel_state" to="diagnostics_toplevel_state" />
+            </node>
+        </group>
+
+        <!-- Battery checking -->
+        <node pkg="dinova_bringup" type="battery_checker" name="battery_checker"/>
+
+        <!-- Vicon Nodes -->
+        <group if="$(arg vicon)">
+            <rosparam ns="vicon/object_specific" command="load" file="$(arg vicon_config_file)"/>
+            <include file="$(find dinova_bringup)/launch/vicon.launch"/>
+            <node pkg="tf" type="static_transform_publisher" name="$(env ROBOT_NAME)_static_tf" args="0 0 0 0 0 0 $(env ROBOT_NAME) $(env ROBOT_NAME)/base_link 0.1" />
+        </group>
+
+        <!-- LIDAR Nodes -->
+        <group if="$(arg lidar)">
+            <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
+            </include>
+        </group>
+
+        <!-- Vicon Publish robot pose to omni_states_vicon -->
+        <node name="dinova_state_publisher" pkg="dinova_control" type="dingo_state_publisher.py" args="" output="screen"/>
 
-    <!-- LIDAR Nodes -->
-    <group if="$(arg lidar)">
-        <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
-        </include>
     </group>
-
-    <!-- Vicon Publish robot pose to omni_states_vicon -->
-    <node name="dinova_state_publisher" pkg="dinova_control" type="dingo_state_publisher.py" args="" output="screen"/>
-
-
 </launch>
diff --git a/dinova_bringup/launch/dinova.launch b/dinova_bringup/launch/dinova.launch
@@ -8,7 +8,7 @@
                         For Gazebo set it to false -->
     <arg name="dingo_real_urdf" default="true" />
     <arg name="load_gripper" default="true" />
-    <arg name="lidar" default="true"/>
+    <arg name="lidar" default="false"/>
     <arg name="vicon" default="true" />
     <arg name="vicon_config_file" default="$(find dinova_bringup)/config/vicon_$(env ROBOT_NAME).yaml"/>
 
@@ -27,7 +27,7 @@
                     lidar:=$(arg lidar)"/>
 
         <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" >
-	    <param name="tf_prefix" value="$(env ROBOT_NAME)"/>
+	        <param name="tf_prefix" value="$(env ROBOT_NAME)"/>
             <remap from="joint_states" to="dinova/joint_states" />
         </node>
 
@@ -45,10 +45,10 @@
         <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter">
             <rosparam file="$(eval optenv('DINGO_MAG_CONFIG', find('dingo_base')+'/config/mag_config_default.yaml'))" />
             <rosparam>
-            gain: 0.1
-            zeta: 0.001
-            publish_tf: false
-            use_magnetic_field_msg: true
+                gain: 0.1
+                zeta: 0.001
+                publish_tf: false
+                use_magnetic_field_msg: true
             </rosparam>
         </node>
 
@@ -61,33 +61,36 @@
         <!-- Diagnostic Aggregator for robot monitor usage -->
         <group if="$(optenv DINGO_OMNI 0)">
             <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
-            <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
-            <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_omni.yaml" />
-            <remap from="/diagnostics" to="diagnostics" />
-            <remap from="/diagnostics_agg" to="diagnostics_agg" />
-            <remap from="/diagnostics_toplevel_state" to="diagnostics_toplevel_state" />
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_omni.yaml" />
+                <remap from="/diagnostics" to="diagnostics" />
+                <remap from="/diagnostics_agg" to="diagnostics_agg" />
+                <remap from="/diagnostics_toplevel_state" to="diagnostics_toplevel_state" />
             </node>
         </group>
         <group unless="$(optenv DINGO_OMNI 0)">
             <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
-            <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
-            <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_diff.yaml" />
-            <remap from="/diagnostics" to="diagnostics" />
-            <remap from="/diagnostics_agg" to="diagnostics_agg" />
-            <remap from="/diagnostics_toplevel_state" to="diagnostics_toplevel_state" />
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_common.yaml" />
+                <rosparam command="load" file="$(find dingo_base)/config/diagnostic_analyzers_diff.yaml" />
+                <remap from="/diagnostics" to="diagnostics" />
+                <remap from="/diagnostics_agg" to="diagnostics_agg" />
+                <remap from="/diagnostics_toplevel_state" to="diagnostics_toplevel_state" />
             </node>
         </group>
 
         <!-- Kinova Nodes -->
         <rosparam command="load" file="$(find dinova_control)/config/config.yaml" />
         <node name="dinova_control" pkg="dinova_control" type="kinova_driver.py" args="" output="screen"/>
         <node name="dinova_state_publisher" pkg="dinova_control" type="dinova_state_publisher.py" args="" output="screen"/>
+
+        <!-- Battery checking -->
+        <node pkg="dinova_bringup" type="battery_checker" name="battery_checker"/>
 
         <!-- Vicon nodes -->
         <group if="$(arg vicon)">
             <rosparam ns="vicon/object_specific" command="load" file="$(arg vicon_config_file)"/>
             <include file="$(find dinova_bringup)/launch/vicon.launch"/>
-	    <node pkg="tf" type="static_transform_publisher" name="$(env ROBOT_NAME)_static_tf" args="0 0 0 0 0 0 $(env ROBOT_NAME) $(env ROBOT_NAME)/base_link 0.1" />
+	        <node pkg="tf" type="static_transform_publisher" name="$(env ROBOT_NAME)_static_tf" args="0 0 0 0 0 0 $(env ROBOT_NAME) $(env ROBOT_NAME)/base_link 0.1" />
         </group>
 
     </group>

diff --git a/dinova_bringup/launch/kinova.launch b/dinova_bringup/launch/kinova.launch
@@ -1,30 +1,35 @@
 <?xml version="1.0"?>
 <launch>
+    <!-- Vicon -->
+    <arg name="vicon" default="false" />
+    <arg name="vicon_config_file" default="$(find dinova_bringup)/config/vicon_$(env ROBOT_NAME).yaml"/>
 
+    <group ns="$(env ROBOT_NAME)">      
+        <param name="vicon/use_vicon" type="bool" value="$(arg vicon)" />
+        <param name="vicon/dingo_topic" type="string" value="/vicon/$(env ROBOT_NAME)" />
 
-    <!-- Robot model -->
-    <param name="robot_description" command="$(find xacro)/xacro '$(find dinova_description)/urdf/kinova.xacro'"/>
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" >
-        <remap from="/joint_states" to="/kinova/joint_states" />
-    </node>
+        <!-- Robot model -->
+        <param name="robot_description" command="$(find xacro)/xacro '$(find dinova_description)/urdf/kinova.xacro'"/>
+        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" >
+            <remap from="joint_states" to="kinova/joint_states" />
+            <param name="tf_prefix" value="$(env ROBOT_NAME)"/>
+        </node>
 
-    <!-- Teleoperation via joystick and rviz interactive markers -->
-    <include file="$(find dingo_control)/launch/teleop.launch" />
+        <!-- Teleoperation via joystick and rviz interactive markers -->
+        <include file="$(find dinova_control)/launch/dingo_teleop.launch" />
 
-     <!-- Vicon -->
-    <arg name="vicon" default="true" />
-    <arg name="vicon_config_file" default="$(find dinova_bringup)/config/vicon_$(env ROBOT_NAME).yaml"/>
-    <param name="/vicon/use_vicon" type="bool" value="$(arg vicon)" />
-    <param name="/vicon/dingo_topic" type="string" value="/vicon/$(env ROBOT_NAME)" />
 
+        <rosparam command="load" file="$(find dinova_control)/config/config.yaml" />
+        <node name="kinova_driver" pkg="dinova_control" type="kinova_driver.py" args="" output="screen"/>
 
-    <rosparam command="load" file="$(find dinova_control)/config/config.yaml" />
-    <node name="kinova_driver" pkg="dinova_control" type="kinova_driver.py" args="" output="screen"/>
+        <!-- Vicon Nodes -->
+        <group if="$(arg vicon)">
+            <rosparam ns="vicon/object_specific" command="load" file="$(arg vicon_config_file)"/>
+            <include file="$(find dinova_bringup)/launch/vicon.launch"/>
+            <!-- TODO: modify this -->
+            <!-- <node pkg="tf" type="static_transform_publisher" name="$(env ROBOT_NAME)_static_tf" args="0 0 0 0 0 0 $(env ROBOT_NAME) $(env ROBOT_NAME)/base_link 0.1" /> -->
+        </group>
 
-    <!-- Vicon Nodes -->
-    <!-- <group if="$(eval arg('vicon') == 'true')">
-        <rosparam ns="vicon/object_specific" command="load" file="$(arg vicon_config_file)"/>
-        <include file="$(find dinova_bringup)/launch/vicon.launch"/>
-    </group> -->
+    </group>
 
 </launch>
diff --git a/dinova_bringup/scripts/battery_checker b/dinova_bringup/scripts/battery_checker
@@ -76,7 +76,7 @@ class BatteryChecker():
 
     def establish_ros_connections(self):
         rospy.Subscriber("battery/status", BatteryState, self._battery_state_callback)
-        self._light_pub = rospy.Publisher("/cmd_lights", Lights, queue_size=1)
+        self._light_pub = rospy.Publisher("cmd_lights", Lights, queue_size=1)
 
     def _battery_state_callback(self, data: BatteryState):
         self._percentage = data.percentage