Update bringup launch file and URDF to support real Mini Pupper (#45)

Co-authored-by: 0nhc <thefoxfoxfox@outlook.com>
mangdangroboticsclub · Mar 4, 2023 · ab4bad7 · ab4bad7
1 parent eb55dc9
commit ab4bad7
Show file tree

Hide file tree

Showing 5 changed files with 132 additions and 29 deletions.
diff --git a/README.md b/README.md
@@ -83,7 +83,7 @@ Note: This step is only for PC
 ```sh
 # Terminal 1
 . ~/ros2_ws/install/setup.bash # setup.zsh if you use zsh instead of bash
-ros2 launch mini_pupper_bringup bringup.launch.py rviz:=true
+ros2 launch mini_pupper_bringup bringup.launch.py joint_hardware_connected:=false rviz:=true
 
 # Terminal 2
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
@@ -124,7 +124,21 @@ ros2 run teleop_twist_keyboard teleop_twist_keyboard
 
 ## 2.2 Mini Pupper
 
-(To be prepared)
+### 2.1.1 Test walk
+
+Note: This step is only for Mini Pupper
+
+Open 2 terminals and ssh login to Mini Pupper on both.
+
+```sh
+# Terminal 1 (ssh)
+. ~/ros2_ws/install/setup.bash # setup.zsh if you use zsh instead of bash
+ros2 launch mini_pupper_bringup bringup.launch.py
+
+# Terminal 2 (ssh)
+ros2 run teleop_twist_keyboard teleop_twist_keyboard
+# Then control Mini Pupper with the keyboard
+```
 
 ## License
 
@@ -150,3 +164,19 @@ limitations under the License.
   * A. No. Ubuntu 22.04 only for now.
 * Q. Is ROS 2 Foxy/Galactic supported?
   * A. No. ROS 2 Humble only for now.
+* Q. `colcon build` shows `1 package had stderr output: mini_pupper_control`.
+  * A. The following warnings can be safely ignored. See [mini_pupper_ros#45](https://github.com/mangdangroboticsclub/mini_pupper_ros/pull/45#discussion_r1104759104) for details.
+  ```
+  Starting >>> mini_pupper_description
+  --- stderr: mini_pupper_control
+  /usr/lib/python3/dist-packages/setuptools/command/easy_install.py:158: EasyInstallDeprecationWarning: easy_install command is deprecated. Use build and pip and other standards-based tools.
+    warnings.warn(
+  /usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.
+    warnings.warn(
+  /usr/lib/python3/dist-packages/pkg_resources/__init__.py:116: PkgResourcesDeprecationWarning: 1.1build1 is an invalid version and will not be supported in a future release
+    warnings.warn(
+  /usr/lib/python3/dist-packages/pkg_resources/__init__.py:116: PkgResourcesDeprecationWarning: 0.1.43ubuntu1 is an invalid version and will not be supported in a future release
+    warnings.warn(
+  ---
+  Finished <<< mini_pupper_control [7.37s]
+  ```
diff --git a/mini_pupper_bringup/launch/bringup.launch.py b/mini_pupper_bringup/launch/bringup.launch.py
@@ -54,6 +54,12 @@ def generate_launch_description():
     rviz_config_path = PathJoinSubstitution(
         [description_package, 'rviz', 'urdf_viewer.rviz']
     )
+    servo_interface_launch_path = PathJoinSubstitution(
+        [FindPackageShare('mini_pupper_control'), 'launch', 'servo_interface.launch.py']
+    )
+    lidar_launch_path = PathJoinSubstitution(
+        [FindPackageShare('mini_pupper_bringup'), 'launch', 'lidar.launch.py']
+    )
 
     robot_name = LaunchConfiguration("robot_name")
     sim = LaunchConfiguration("sim")
@@ -74,13 +80,13 @@ def generate_launch_description():
 
     declare_rviz = DeclareLaunchArgument(
             name='rviz',
-            default_value='true',
+            default_value='false',
             description='Run rviz'
         )
 
     declare_hardware_connected = DeclareLaunchArgument(
             name='joint_hardware_connected',
-            default_value='false',
+            default_value='true',
             description='Set to true if connected to a physical robot'
         )
 
@@ -94,28 +100,40 @@ def generate_launch_description():
         )
 
     bringup_launch = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(bringup_launch_path),
-            launch_arguments={
-                "use_sim_time": sim,
-                "robot_name": robot_name,
-                "gazebo": sim,
-                "rviz": "false",  # set always false to launch RViz2 with costom .rviz file
-                "joint_hardware_connected": joint_hardware_connected,
-                "publish_foot_contacts": "true",
-                "close_loop_odom": "true",
-                "joint_controller_topic": "joint_group_effort_controller/joint_trajectory",
-                "joints_map_path": joints_config_path,
-                "links_map_path": links_config_path,
-                "gait_config_path": gait_config_path,
-                "description_path": description_path
-            }.items(),
-        )
+        PythonLaunchDescriptionSource(bringup_launch_path),
+        launch_arguments={
+            "use_sim_time": sim,
+            "robot_name": robot_name,
+            "gazebo": sim,
+            "rviz": "false",  # set always false to launch RViz2 with costom .rviz file
+            "joint_hardware_connected": joint_hardware_connected,
+            "publish_foot_contacts": "true",
+            "close_loop_odom": "true",
+            "joint_controller_topic": "joint_group_effort_controller/joint_trajectory",
+            "joints_map_path": joints_config_path,
+            "links_map_path": links_config_path,
+            "gait_config_path": gait_config_path,
+            "description_path": description_path
+        }.items(),
+    )
+
+    servo_interface_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(servo_interface_launch_path),
+        condition=IfCondition(joint_hardware_connected),
+    )
+
+    lidar_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(lidar_launch_path),
+        condition=IfCondition(joint_hardware_connected),
+    )
 
     return LaunchDescription([
         declare_robot_name,
         declare_sim,
         declare_rviz,
         declare_hardware_connected,
         rviz2_node,
-        bringup_launch
+        bringup_launch,
+        servo_interface_launch,
+        lidar_launch,
     ])
diff --git a/mini_pupper_bringup/launch/lidar.launch.py b/mini_pupper_bringup/launch/lidar.launch.py
@@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2022-2023 MangDang
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='ldlidar_stl_ros2',
+            executable='ldlidar_stl_ros2_node',
+            name='LD06',
+            output='screen',
+            parameters=[
+                {'product_name': 'LDLiDAR_LD06'},
+                {'topic_name': 'scan'},
+                {'frame_id': 'lidar_link'},
+                {'port_name': '/dev/ttyUSB0'},
+                {'port_baudrate': 230400},
+                {'laser_scan_dir': True},
+                {'enable_angle_crop_func': False},
+                {'angle_crop_min': 135.0},
+                {'angle_crop_max': 225.0}],
+        ),
+    ])
diff --git a/mini_pupper_description/urdf/mini_pupper_description.urdf b/mini_pupper_description/urdf/mini_pupper_description.urdf
@@ -74,12 +74,12 @@
   </joint>
   <link name="lidar">
     <inertial>
-      <origin rpy="0 0 0" xyz="1.56706411611127E-05 -5.16565672073303E-13 -0.0191835739440227"/>
+      <origin rpy="0 0 -1.57" xyz="1.56706411611127E-05 -5.16565672073303E-13 -0.0191835739440227"/>
       <mass value="0.0001"/>
       <inertia ixx="8.74731426973985E-06" ixy="-6.66781590704156E-08" ixz="1.89196685410159E-11" iyy="8.70097486591694E-06" iyz="-1.13397577799603E-17" izz="9.98821295317784E-06"/>
     </inertial>
     <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <origin rpy="0 0 -1.57" xyz="0 0 0"/>
       <geometry>
         <mesh filename="file://$(find mini_pupper_description)/meshes/lidar.STL"/>
       </geometry>
@@ -88,14 +88,14 @@
       </material>
     </visual>
     <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <origin rpy="0 0 -1.57" xyz="0 0 0"/>
       <geometry>
         <mesh filename="file://$(find mini_pupper_description)/meshes/lidar.STL"/>
       </geometry>
     </collision>
   </link>
   <joint name="base_lidar" type="fixed">
-    <origin rpy="0 0 0" xyz="-0.070475 0.0001 0.08"/>
+    <origin rpy="0 0 1.57" xyz="-0.070475 0.0001 0.08"/>
     <parent link="base_link"/>
     <child link="lidar"/>
     <axis xyz="0 0 0"/>

diff --git a/mini_pupper_description/urdf/mini_pupper_description.urdf.xacro b/mini_pupper_description/urdf/mini_pupper_description.urdf.xacro
@@ -105,7 +105,7 @@
     <inertial>
       <origin
         xyz="1.56706411611127E-05 -5.16565672073303E-13 -0.0191835739440227"
-        rpy="0 0 0" />
+        rpy="0 0 -1.57" />
       <mass
         value="0.0001" />
       <inertia
@@ -119,7 +119,7 @@
     <visual>
       <origin
         xyz="0 0 0"
-        rpy="0 0 0" />
+        rpy="0 0 -1.57" />
       <geometry>
         <mesh
           filename="file://$(find mini_pupper_description)/meshes/lidar.STL" />
@@ -133,7 +133,7 @@
     <collision>
       <origin
         xyz="0 0 0"
-        rpy="0 0 0" />
+        rpy="0 0 -1.57" />
       <geometry>
         <mesh
           filename="file://$(find mini_pupper_description)/meshes/lidar.STL" />
@@ -145,14 +145,28 @@
     type="fixed">
     <origin
       xyz="-0.070475 0.0001 0.08"
-      rpy="0 0 0" />
+      rpy="0 0 1.57" />
     <parent
       link="base_link" />
     <child
       link="lidar" />
     <axis
       xyz="0 0 0" />
   </joint>
+  <link name="lidar_link" />
+  <joint
+    name="lidar_link_joint"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="lidar" />
+    <child
+      link="lidar_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
   <link
     name="lf1">
     <inertial>