robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

add skill launch, refactor pick-place pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-11-30 13:47:33 +03:00
parent 034e172f62
commit d72c06efea
14 changed files with 1032 additions and 191 deletions
Download patch file Download diff file Expand all files Collapse all files

51
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -6,48 +6,20 @@ from ament_index_python.packages import get_package_share_directory
import os
import yaml
import math
def construct_angle_radians(loader, node):
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
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:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
from rbs_launch_utils.launch_common import load_yaml
def generate_launch_description():
bt_config = os.path.join(
get_package_share_directory('rbs_bt_executor'),
'config',
'params.yaml'
)
points = load_yaml(
package_name="rbs_bt_executor",
file_path="config/points.yaml"
)
# bt_config = os.path.join(
# get_package_share_directory('rbs_bt_executor'),
# 'config',
# 'params.yaml'
# )
# points = load_yaml(
# package_name="rbs_bt_executor",
# file_path="config/points.yaml"
# )
gripperPoints = load_yaml(
package_name="rbs_bt_executor",
@ -76,7 +48,6 @@ def generate_launch_description():
return LaunchDescription([
Node(
package='behavior_tree',
namespace='',
executable='bt_engine',
#prefix=['xterm -e gdb -ex run --args'],
parameters=[
@ -88,7 +59,7 @@ def generate_launch_description():
"rbs_add_planning_scene_object",
"rbs_pose_estimation"
]},
gripperPoints,
# gripperPoints,
robot_description_semantic
]
)