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

refactoring run

Browse source
This commit is contained in:
shalenikol 2023-11-16 13:15:27 +03:00
parent c5f37c587f
commit 4decc40c88
6 changed files with 77 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rbs_bringup/launch/bringup.launch.py
View file

@ -153,7 +153,7 @@ def generate_launch_description():
DeclareLaunchArgument("launch_perception", default_value="false", description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner", default_value="true", description="Launch task_planner?")
DeclareLaunchArgument("launch_task_planner", default_value="false", description="Launch task_planner?")
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers", default_value="false", description="Load cartesian controllers?")

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

@ -8,98 +8,88 @@ 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_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 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)
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:
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
# 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"
# )
# gripperPoints = load_yaml(
# package_name="rbs_bt_executor",
# file_path="config/gripperPositions.yaml")
# robot_description_semantic_content = Command(
# [
# PathJoinSubstitution([FindExecutable(name="xacro")]),
# " ",
# PathJoinSubstitution(
# [FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
# ),
# " ",
# "name:=",
# "ur",
# " ",
# "prefix:=",
# "",
# " ",
# ]
# )
# robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
# return LaunchDescription([
# Node(
# package='behavior_tree',
# namespace='',
# executable='bt_engine',
# #prefix=['xterm -e gdb -ex run --args'],
# parameters=[
# {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
# {'plugins':['rbs_skill_move_topose_bt_action_client', "rbs_skill_get_pick_place_pose_service_client", "rbs_skill_gripper_move_bt_action_client", "rbs_skill_move_joint_state", "rbs_add_planning_scene_object"]},
# gripperPoints,
# robot_description_semantic
# ]
# )
# ])
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
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"
)
gripperPoints = load_yaml(
package_name="rbs_bt_executor",
file_path="config/gripperPositions.yaml")
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
),
" ",
"name:=",
"ur",
" ",
"prefix:=",
"",
" ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
return LaunchDescription([
Node(
package='behavior_tree',
namespace='',
executable='bt_engine',
prefix=['xterm -e gdb -ex run --args'],
#prefix=['xterm -e gdb -ex run --args'],
parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/6Dpe_test.xml')},
{'plugins':["rbs_pose_estimation"]},
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
{'plugins':["rbs_skill_move_topose_bt_action_client",
"rbs_skill_get_pick_place_pose_service_client",
"rbs_skill_gripper_move_bt_action_client",
"rbs_skill_move_joint_state",
"rbs_add_planning_scene_object",
"rbs_pose_estimation"
]},
gripperPoints,
robot_description_semantic
]
)
])

2
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -18,7 +18,7 @@ public:
: BtService<PoseEstimationSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/image_sub2");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/pose_estimation");
while (!_set_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {

2
rbs_perception/config/pose_estimation_config.json
View file

@ -1,5 +1,5 @@
{
"nodeName":"image_sub2",
"nodeName":"pose_estimation",
"topicImage":"/outer_rgbd_camera/image",
"topicCameraInfo":"/outer_rgbd_camera/camera_info",
"topicDepth":"/outer_rgbd_camera/depth_image",

8
rbs_perception/launch/perception.launch.py
View file

@ -1,3 +1,4 @@
import os
from launch_ros.actions import Node
from ur_moveit_config.launch_common import load_yaml
from launch import LaunchDescription
@ -16,12 +17,13 @@ def generate_launch_description():
points_params
]
)
grasp_marker = Node(
pose_estimation = Node(
package="rbs_perception",
executable="grasp_marker_publish.py",
executable="pose_estimation_lifecycle.py",
)
nodes_to_start = [
grasp_pose_loader,
grasp_marker
pose_estimation
]
return LaunchDescription(declared_arguments + nodes_to_start)

2
rbs_perception/scripts/pose_estimation_lifecycle.py
View file

@ -70,7 +70,7 @@ class PoseEstimator(Node):
self.tf2_send_pose = 0
self.megapose_model = None
self.nodeName = node_name #"image_sub2"
self.nodeName = node_name
self.topicImage = "/outer_rgb_camera/image"
self.topicCameraInfo = "/outer_rgb_camera/camera_info"
self.topicDepth = "/outer_rgbd_camera/depth_image"