68 lines
2.2 KiB
Python
68 lines
2.2 KiB
Python
import os
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def generate_launch_description():
|
|
user_debug_parameter_name = "user_debug"
|
|
user_debug = LaunchConfiguration(user_debug_parameter_name)
|
|
# prefix = LaunchConfiguration("prefix", default="go2/")
|
|
|
|
go2_xacro_file = os.path.join(
|
|
get_package_share_directory("go2_description"), "xacro", "robot.xacro"
|
|
)
|
|
robot_description = Command(
|
|
[FindExecutable(name="xacro"), " ", go2_xacro_file, " DEBUG:=", user_debug]
|
|
)
|
|
|
|
rviz_file = os.path.join(
|
|
get_package_share_directory("go2_description"), "launch", "visualize_go2.rviz"
|
|
)
|
|
|
|
return LaunchDescription(
|
|
[
|
|
DeclareLaunchArgument(
|
|
user_debug_parameter_name,
|
|
default_value="false",
|
|
description="debug or not",
|
|
),
|
|
Node(
|
|
package="robot_state_publisher",
|
|
executable="robot_state_publisher",
|
|
name="robot_state_publisher",
|
|
output="screen",
|
|
parameters=[{"robot_description": robot_description}],
|
|
remappings=[
|
|
("/joint_states", "/go2/joint_states"),
|
|
("/robot_description", "/go2/robot_description"),
|
|
],
|
|
),
|
|
Node(
|
|
package="rviz2",
|
|
executable="rviz2",
|
|
name="rviz2",
|
|
arguments=["--display-config", rviz_file],
|
|
),
|
|
Node(
|
|
package="tf2_ros",
|
|
executable="static_transform_publisher",
|
|
arguments=[
|
|
"0.15",
|
|
"0",
|
|
"0.15",
|
|
"0",
|
|
"0",
|
|
"0.707107",
|
|
"0.707107",
|
|
"/trunk",
|
|
"/go2/hesai_lidar",
|
|
],
|
|
name="static_tf_pub_trunk_to_lidar",
|
|
),
|
|
]
|
|
)
|