From 72154ee355bdc9dd5aa27065f1580f539f5f6333 Mon Sep 17 00:00:00 2001
From: yangwei <youyangwei1@xiaomi.com>
Date: Tue, 9 Jan 2024 09:36:15 +0800
Subject: [PATCH] [Modify]: add cyberdog_tf_publish launch file for visualizing
 the robot state when running on the real robot

JIRA-ID: N/A

1. add cyberdog_tf_publish launch file;
2. remove the dependancy of gazebo_msgs in cyberdog_visual package;

Signed-off-by: yangwei <youyangwei1@xiaomi.com>
---
 cyberdog_visual/CMakeLists.txt                |  2 -
 .../launch/cyberdog_tf_publish.launch.py      | 80 +++++++++++++++++++
 cyberdog_visual/package.xml                   |  1 -
 3 files changed, 80 insertions(+), 3 deletions(-)
 create mode 100644 cyberdog_visual/launch/cyberdog_tf_publish.launch.py

diff --git a/cyberdog_visual/CMakeLists.txt b/cyberdog_visual/CMakeLists.txt
index 9faa307..b9e8ed3 100644
--- a/cyberdog_visual/CMakeLists.txt
+++ b/cyberdog_visual/CMakeLists.txt
@@ -22,7 +22,6 @@ find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(gazebo_msgs REQUIRED)
 find_package(lcm REQUIRED)
 find_package(urdf REQUIRED)
 set(dependencies
@@ -33,7 +32,6 @@ set(dependencies
   tf2_geometry_msgs
   tf2_ros
   sensor_msgs
-  gazebo_msgs
   urdf
   lcm
   Eigen3
diff --git a/cyberdog_visual/launch/cyberdog_tf_publish.launch.py b/cyberdog_visual/launch/cyberdog_tf_publish.launch.py
new file mode 100644
index 0000000..e120a76
--- /dev/null
+++ b/cyberdog_visual/launch/cyberdog_tf_publish.launch.py
@@ -0,0 +1,80 @@
+import os
+import launch
+import xacro
+from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def launch_setup(context, *args, **kwargs):
+    # config
+    hang_robot = LaunchConfiguration('hang_robot').perform(context)
+    use_lidar = LaunchConfiguration('use_lidar').perform(context)
+    rname = LaunchConfiguration('rname').perform(context)
+    use_sim_time=LaunchConfiguration('use_sim_time')
+
+    # path
+    description_share = FindPackageShare(
+        package=rname+'_description').find(rname+'_description')
+    visual_share = FindPackageShare(
+        package='cyberdog_visual').find('cyberdog_visual')
+
+    # urdf
+    xacro_path = os.path.join(description_share, 'xacro/robot.xacro')
+    urdf_contents = xacro.process_file(xacro_path, mappings={
+                                       'DEBUG': hang_robot, 'USE_LIDAR': use_lidar}).toprettyxml(indent='  ')
+
+    # joint_state_publisher
+    joint_state_node = Node(
+        package='cyberdog_visual',
+        executable='cyberdog_visual',
+        name='cyberdog_visual',
+        output='screen',
+        parameters=[{
+                    'robot_description': urdf_contents,
+                    'publish_frequency': 50.0,
+                    'joint_state_topic': 'joint_states',
+                    'use_sim_time': use_sim_time,
+                    'use_state_estimator': False
+                    }
+                    ]
+    )
+
+    # robot_state_publisher
+    robot_state_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[
+                {
+                    'robot_description': urdf_contents,
+                    'publish_frequency': 50.0,
+                    'use_sim_time': use_sim_time
+                }
+        ]
+    )
+
+    return [joint_state_node, robot_state_node]
+
+def generate_launch_description():
+    return launch.LaunchDescription([
+        DeclareLaunchArgument(
+            name='use_sim_time',
+            default_value='false'
+        ),
+        DeclareLaunchArgument(
+            name='hang_robot',
+            default_value='false'
+        ),
+        DeclareLaunchArgument(
+            name='use_lidar',
+            default_value='false'
+        ),
+        DeclareLaunchArgument(
+            name='rname',
+            default_value='cyberdog'
+        ),
+        OpaqueFunction(function=launch_setup)
+    ])
diff --git a/cyberdog_visual/package.xml b/cyberdog_visual/package.xml
index b89aac1..75feafd 100644
--- a/cyberdog_visual/package.xml
+++ b/cyberdog_visual/package.xml
@@ -14,7 +14,6 @@
     <depend>tf2_ros</depend>
     <depend>tf2_geometry_msgs</depend>
     <depend>sensor_msgs</depend>
-    <depend>gazebo_msgs</depend>
     <depend>urdf</depend>
 
     <export>