mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
56 lines
1.8 KiB
Python
56 lines
1.8 KiB
Python
import os
|
|
import yaml
|
|
from launch import LaunchDescription
|
|
from launch_ros.actions import Node
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
|
|
def load_file(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
with open(absolute_file_path, "r") as file:
|
|
return file.read()
|
|
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
|
return None
|
|
|
|
|
|
def load_yaml(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
with open(absolute_file_path, "r") as file:
|
|
return yaml.safe_load(file)
|
|
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
|
return None
|
|
|
|
|
|
def generate_launch_description():
|
|
# Get URDF and SRDF
|
|
robot_description_config = load_file(
|
|
"moveit_resources_panda_description", "urdf/panda.urdf"
|
|
)
|
|
robot_description = {"robot_description": robot_description_config}
|
|
|
|
robot_description_semantic_config = load_file(
|
|
"moveit_resources_panda_moveit_config", "config/panda.srdf"
|
|
)
|
|
robot_description_semantic = {
|
|
"robot_description_semantic": robot_description_semantic_config
|
|
}
|
|
|
|
kinematics_yaml = load_yaml(
|
|
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
|
|
)
|
|
|
|
cartesian_task = Node(
|
|
package="moveit_task_constructor_demo",
|
|
executable="ik_clearance_cost",
|
|
output="screen",
|
|
parameters=[robot_description, robot_description_semantic, kinematics_yaml],
|
|
)
|
|
|
|
return LaunchDescription([cartesian_task])
|