71 lines
2.2 KiB
Python
71 lines
2.2 KiB
Python
|
import xacro
|
||
|
|
from pathlib import Path
|
||
|
|
|
||
|
|
from launch import LaunchDescription
|
||
|
|
import launch_ros
|
||
|
|
from launch_ros.actions import Node
|
||
|
|
from launch_ros.substitutions import FindPackageShare
|
||
|
|
|
||
|
|
def generate_launch_description():
|
||
|
|
pkg_share = Path(
|
||
|
|
launch_ros.substitutions.FindPackageShare(package="openarm_bimanual_description").find(
|
||
|
|
"openarm_bimanual_description"
|
||
|
|
)
|
||
|
|
)
|
||
|
|
default_model_path = pkg_share / "urdf/openarm_bimanual.urdf.xacro"
|
||
|
|
default_rviz_config_path = pkg_share / "rviz/robot_description.rviz"
|
||
|
|
|
||
|
|
# Parse xacro to get robot_description XML
|
||
|
|
doc = xacro.process_file(str(default_model_path))
|
||
|
|
robot_description_xml = doc.toxml()
|
||
|
|
|
||
|
|
# Directly read udrf file (no xacro)
|
||
|
|
# robot_description_xml = Path(default_model_path).read_text()
|
||
|
|
|
||
|
|
# Node to publish joint states from /joint_states topic or GUI
|
||
|
|
joint_state_publisher_node = Node(
|
||
|
|
package="joint_state_publisher",
|
||
|
|
executable="joint_state_publisher",
|
||
|
|
name="joint_state_publisher",
|
||
|
|
output="screen",
|
||
|
|
parameters=[{"use_sim_time": False}],
|
||
|
|
)
|
||
|
|
|
||
|
|
# Node to publish robot state TFs based on robot_description and /joint_states
|
||
|
|
robot_state_publisher_node = Node(
|
||
|
|
package="robot_state_publisher",
|
||
|
|
executable="robot_state_publisher",
|
||
|
|
name="robot_state_publisher",
|
||
|
|
output="screen",
|
||
|
|
parameters=[
|
||
|
|
{"robot_description": robot_description_xml,
|
||
|
|
"use_sim_time": False}
|
||
|
|
],
|
||
|
|
)
|
||
|
|
|
||
|
|
# RViz node to visualize the robot model and TF
|
||
|
|
rviz_node = Node(
|
||
|
|
package="rviz2",
|
||
|
|
executable="rviz2",
|
||
|
|
name="rviz2",
|
||
|
|
output="screen",
|
||
|
|
arguments=["-d", str(default_rviz_config_path)],
|
||
|
|
parameters=[{"use_sim_time": False}],
|
||
|
|
)
|
||
|
|
|
||
|
|
joint_state_merger_node = Node(
|
||
|
|
package="openarm_bimanual_description",
|
||
|
|
executable="joint_state_merger.py",
|
||
|
|
name="joint_state_merger",
|
||
|
|
output="screen",
|
||
|
|
parameters=[{"urdf_path": str(default_model_path)}],
|
||
|
|
)
|
||
|
|
|
||
|
|
|
||
|
|
return LaunchDescription([
|
||
|
|
#joint_state_publisher_node,
|
||
|
|
robot_state_publisher_node,
|
||
|
|
joint_state_merger_node,
|
||
|
|
rviz_node,
|
||
|
|
])
|