From 8356d149ae54cab00a81d637430661b610f2a5ca Mon Sep 17 00:00:00 2001 From: Yadu Date: Tue, 14 Nov 2023 14:17:39 +0800 Subject: [PATCH] Use parent node options (#24) Signed-off-by: Yadunund --- nexus_motion_planner/CMakeLists.txt | 1 + .../launch/demo_planner_server.launch.py | 2 +- .../src/motion_planner_server.cpp | 33 ++----------------- 3 files changed, 5 insertions(+), 31 deletions(-) diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index 9c69e8d..2ed33c7 100644 --- a/nexus_motion_planner/CMakeLists.txt +++ b/nexus_motion_planner/CMakeLists.txt @@ -33,6 +33,7 @@ set (motion_planner_server_dependencies ) set (test_request_dependencies + moveit_msgs nexus_endpoints geometry_msgs rclcpp diff --git a/nexus_motion_planner/launch/demo_planner_server.launch.py b/nexus_motion_planner/launch/demo_planner_server.launch.py index 292c3ae..ebf13e4 100644 --- a/nexus_motion_planner/launch/demo_planner_server.launch.py +++ b/nexus_motion_planner/launch/demo_planner_server.launch.py @@ -197,7 +197,7 @@ def launch_setup(context, *args, **kwargs): executable="static_transform_publisher", name="motion_planner_static_transform_publisher_base_link_t_item", output="log", - arguments=["0.3", "0.2", "0.1", "0.0", "0.0", "0.0", "base_link", "item"], + arguments=["0.6", "0.2", "0.5", "3.14", "0.0", "3.14", "base_link", "item"], condition=IfCondition(publish_item), ) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 1ecd6f1..9c53046 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -20,17 +20,14 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) { RCLCPP_INFO(this->get_logger(), "Motion Planner Server is running..."); - auto internal_node_options = rclcpp::NodeOptions(); - internal_node_options.automatically_declare_parameters_from_overrides(true); - internal_node_options.use_global_arguments(false); _internal_node = std::make_shared( - "motion_planner_server_internal_node", internal_node_options); + "motion_planner_server_internal_node", options); _spin_thread = std::thread( - [this]() + [node = _internal_node]() { while (rclcpp::ok()) { - rclcpp::spin_some(_internal_node); + rclcpp::spin_some(node); } }); @@ -70,30 +67,6 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) this->declare_parameter("default_group_name", "manipulator"); _group_names.insert({name, std::move(group_name)}); - const std::string description_param_name = _use_namespace ? - name + ".robot_description" : "robot_description"; - const auto description_param = - this->declare_parameter( - description_param_name, - rclcpp::ParameterType::PARAMETER_STRING); - - // Push robot_description parameter to internal node - _internal_node->declare_parameter( - description_param_name, - description_param); - - const std::string description_semantic_param_name = _use_namespace ? - name + ".robot_description_semantic" : "robot_description_semantic"; - const auto description_semantic_param = - this->declare_parameter( - description_semantic_param_name, - rclcpp::ParameterType::PARAMETER_STRING); - - // Push robot_description_semantic parameter to internal node - _internal_node->declare_parameter( - description_semantic_param_name, - description_semantic_param); - } RCLCPP_INFO( this->get_logger(),