From 6fd66f95159c083cb51421217453a818d5d472c4 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 11 Oct 2022 16:35:58 +0800 Subject: [PATCH 1/4] Namespace lift topics Signed-off-by: Luca Della Vedova --- .../include/rmf_building_sim_common/lift_common.hpp | 9 +++++++-- rmf_building_sim_common/src/lift_common.cpp | 11 ++++++++--- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp index ca601e4..5e079f9 100644 --- a/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp +++ b/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp @@ -115,7 +115,8 @@ class LiftCommon std::string, std::vector> floor_name_to_cabin_door_name, std::unordered_map shaft_door_states, std::unordered_map cabin_door_states, - std::string initial_floor_name); + std::string initial_floor_name, + const std::string& lift_namespace); double get_step_velocity(const double dt, const double position, const double velocity); @@ -147,6 +148,7 @@ std::unique_ptr LiftCommon::make( { MotionParams cabin_motion_params; std::string joint_name; + std::string lift_namespace; std::vector floor_names; std::unordered_map floor_name_to_elevation; std::unordered_map LiftCommon::make( cabin_motion_params.dx_min); get_sdf_param_if_available(sdf_clone, "f_max_cabin", cabin_motion_params.f_max); + get_sdf_param_if_available(sdf_clone, "namespace", + lift_namespace); if (!get_sdf_param_required(sdf_clone, "cabin_joint_name", joint_name)) return nullptr; @@ -256,7 +260,8 @@ std::unique_ptr LiftCommon::make( floor_name_to_cabin_door_name, shaft_door_states, cabin_door_states, - initial_floor_name)); + initial_floor_name, + lift_namespace)); return lift; } diff --git a/rmf_building_sim_common/src/lift_common.cpp b/rmf_building_sim_common/src/lift_common.cpp index 9a634ea..28dd84e 100644 --- a/rmf_building_sim_common/src/lift_common.cpp +++ b/rmf_building_sim_common/src/lift_common.cpp @@ -184,7 +184,8 @@ LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, std::string, std::vector> floor_name_to_cabin_door_name, std::unordered_map shaft_door_states, std::unordered_map cabin_door_states, - std::string initial_floor_name) + std::string initial_floor_name, + const std::string& lift_namespace) : _ros_node(node), _lift_name(lift_name), _cabin_joint_name(joint_name), @@ -202,15 +203,19 @@ LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, for (const auto& it : _floor_name_to_elevation) std::cout << it.first << " | " << it.second << std::endl; + std::string ns; + if (lift_namespace.size() > 0) + ns = "/" + lift_namespace; // initialize pub & sub _lift_state_pub = _ros_node->create_publisher( - "/lift_states", rclcpp::SystemDefaultsQoS()); + ns + "/lift_states", rclcpp::SystemDefaultsQoS()); + // TODO(luca) remove adapter door request and make lift command its own doors _door_request_pub = _ros_node->create_publisher( "/adapter_door_requests", rclcpp::SystemDefaultsQoS()); _lift_request_sub = _ros_node->create_subscription( - "/lift_requests", rclcpp::SystemDefaultsQoS(), + ns + "/lift_requests", rclcpp::SystemDefaultsQoS(), [&](LiftRequest::UniquePtr msg) { if (msg->lift_name != _lift_name) From 43d8571cd6cdc847bf6828bc861d7db9b2bf3912 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 12 Oct 2022 14:25:56 +0800 Subject: [PATCH 2/4] Namespace doors Signed-off-by: Luca Della Vedova --- .../include/rmf_building_sim_common/door_common.hpp | 13 ++++++++++--- rmf_building_sim_common/src/door_common.cpp | 11 ++++++++--- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp index 0476cfa..feeaddb 100644 --- a/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp +++ b/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp @@ -126,7 +126,8 @@ class DoorCommon DoorCommon(const std::string& door_name, rclcpp::Node::SharedPtr node, const MotionParams& params, - const Doors& doors); + const Doors& doors, + const std::string& door_namespace); bool all_doors_open(); @@ -162,11 +163,13 @@ std::shared_ptr DoorCommon::make( auto sdf_clone = sdf->Clone(); MotionParams params; + std::string door_namespace; get_sdf_param_if_available(sdf_clone, "v_max_door", params.v_max); get_sdf_param_if_available(sdf_clone, "a_max_door", params.a_max); get_sdf_param_if_available(sdf_clone, "a_nom_door", params.a_nom); get_sdf_param_if_available(sdf_clone, "dx_min_door", params.dx_min); get_sdf_param_if_available(sdf_clone, "f_max_door", params.f_max); + get_sdf_param_if_available(sdf_clone, "namespace", door_namespace); auto door_element = sdf_clone; std::string left_door_joint_name; @@ -263,7 +266,8 @@ std::shared_ptr DoorCommon::make( door_name, node, params, - doors)); + doors, + door_namespace)); return door_common; @@ -281,17 +285,20 @@ std::shared_ptr DoorCommon::make( auto sdf_clone = sdf->Clone(); MotionParams params; + std::string door_namespace; get_sdf_param_if_available(sdf_clone, "v_max_door", params.v_max); get_sdf_param_if_available(sdf_clone, "a_max_door", params.a_max); get_sdf_param_if_available(sdf_clone, "a_nom_door", params.a_nom); get_sdf_param_if_available(sdf_clone, "dx_min_door", params.dx_min); get_sdf_param_if_available(sdf_clone, "f_max_door", params.f_max); + get_sdf_param_if_available(sdf_clone, "namespace", door_namespace); std::shared_ptr door_common(new DoorCommon( door_name, node, params, - doors)); + doors, + door_namespace)); return door_common; } diff --git a/rmf_building_sim_common/src/door_common.cpp b/rmf_building_sim_common/src/door_common.cpp index 729805e..7080ab3 100644 --- a/rmf_building_sim_common/src/door_common.cpp +++ b/rmf_building_sim_common/src/door_common.cpp @@ -63,7 +63,8 @@ void DoorCommon::publish_state(const uint32_t door_value, DoorCommon::DoorCommon(const std::string& door_name, rclcpp::Node::SharedPtr node, const MotionParams& params, - const Doors& doors) + const Doors& doors, + const std::string& door_namespace) : _ros_node(std::move(node)), _params(params), _doors(doors) @@ -71,11 +72,15 @@ DoorCommon::DoorCommon(const std::string& door_name, _state.door_name = door_name; _request.requested_mode.value = DoorMode::MODE_CLOSED; + std::string ns; + if (door_namespace.size() > 0) + ns = "/" + door_namespace; + _door_state_pub = _ros_node->create_publisher( - "/door_states", rclcpp::SystemDefaultsQoS()); + ns + "/door_states", rclcpp::SystemDefaultsQoS()); _door_request_sub = _ros_node->create_subscription( - "/door_requests", rclcpp::SystemDefaultsQoS(), + ns + "/door_requests", rclcpp::SystemDefaultsQoS(), [&](DoorRequest::UniquePtr msg) { if (msg->door_name == _state.door_name) From 79133531331021100feb7788f64dee7d0b7560a0 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 8 Dec 2022 12:56:39 +0800 Subject: [PATCH 3/4] Force infrastructure plugins to be namespaced Signed-off-by: Luca Della Vedova --- .../include/rmf_building_sim_common/door_common.hpp | 13 +++---------- .../include/rmf_building_sim_common/lift_common.hpp | 9 ++------- rmf_building_sim_common/src/door_common.cpp | 11 +++-------- rmf_building_sim_common/src/lift_common.cpp | 10 +++------- 4 files changed, 11 insertions(+), 32 deletions(-) diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp index feeaddb..0476cfa 100644 --- a/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp +++ b/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp @@ -126,8 +126,7 @@ class DoorCommon DoorCommon(const std::string& door_name, rclcpp::Node::SharedPtr node, const MotionParams& params, - const Doors& doors, - const std::string& door_namespace); + const Doors& doors); bool all_doors_open(); @@ -163,13 +162,11 @@ std::shared_ptr DoorCommon::make( auto sdf_clone = sdf->Clone(); MotionParams params; - std::string door_namespace; get_sdf_param_if_available(sdf_clone, "v_max_door", params.v_max); get_sdf_param_if_available(sdf_clone, "a_max_door", params.a_max); get_sdf_param_if_available(sdf_clone, "a_nom_door", params.a_nom); get_sdf_param_if_available(sdf_clone, "dx_min_door", params.dx_min); get_sdf_param_if_available(sdf_clone, "f_max_door", params.f_max); - get_sdf_param_if_available(sdf_clone, "namespace", door_namespace); auto door_element = sdf_clone; std::string left_door_joint_name; @@ -266,8 +263,7 @@ std::shared_ptr DoorCommon::make( door_name, node, params, - doors, - door_namespace)); + doors)); return door_common; @@ -285,20 +281,17 @@ std::shared_ptr DoorCommon::make( auto sdf_clone = sdf->Clone(); MotionParams params; - std::string door_namespace; get_sdf_param_if_available(sdf_clone, "v_max_door", params.v_max); get_sdf_param_if_available(sdf_clone, "a_max_door", params.a_max); get_sdf_param_if_available(sdf_clone, "a_nom_door", params.a_nom); get_sdf_param_if_available(sdf_clone, "dx_min_door", params.dx_min); get_sdf_param_if_available(sdf_clone, "f_max_door", params.f_max); - get_sdf_param_if_available(sdf_clone, "namespace", door_namespace); std::shared_ptr door_common(new DoorCommon( door_name, node, params, - doors, - door_namespace)); + doors)); return door_common; } diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp index 5e079f9..ca601e4 100644 --- a/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp +++ b/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp @@ -115,8 +115,7 @@ class LiftCommon std::string, std::vector> floor_name_to_cabin_door_name, std::unordered_map shaft_door_states, std::unordered_map cabin_door_states, - std::string initial_floor_name, - const std::string& lift_namespace); + std::string initial_floor_name); double get_step_velocity(const double dt, const double position, const double velocity); @@ -148,7 +147,6 @@ std::unique_ptr LiftCommon::make( { MotionParams cabin_motion_params; std::string joint_name; - std::string lift_namespace; std::vector floor_names; std::unordered_map floor_name_to_elevation; std::unordered_map LiftCommon::make( cabin_motion_params.dx_min); get_sdf_param_if_available(sdf_clone, "f_max_cabin", cabin_motion_params.f_max); - get_sdf_param_if_available(sdf_clone, "namespace", - lift_namespace); if (!get_sdf_param_required(sdf_clone, "cabin_joint_name", joint_name)) return nullptr; @@ -260,8 +256,7 @@ std::unique_ptr LiftCommon::make( floor_name_to_cabin_door_name, shaft_door_states, cabin_door_states, - initial_floor_name, - lift_namespace)); + initial_floor_name)); return lift; } diff --git a/rmf_building_sim_common/src/door_common.cpp b/rmf_building_sim_common/src/door_common.cpp index 7080ab3..d14df7c 100644 --- a/rmf_building_sim_common/src/door_common.cpp +++ b/rmf_building_sim_common/src/door_common.cpp @@ -63,8 +63,7 @@ void DoorCommon::publish_state(const uint32_t door_value, DoorCommon::DoorCommon(const std::string& door_name, rclcpp::Node::SharedPtr node, const MotionParams& params, - const Doors& doors, - const std::string& door_namespace) + const Doors& doors) : _ros_node(std::move(node)), _params(params), _doors(doors) @@ -72,15 +71,11 @@ DoorCommon::DoorCommon(const std::string& door_name, _state.door_name = door_name; _request.requested_mode.value = DoorMode::MODE_CLOSED; - std::string ns; - if (door_namespace.size() > 0) - ns = "/" + door_namespace; - _door_state_pub = _ros_node->create_publisher( - ns + "/door_states", rclcpp::SystemDefaultsQoS()); + "sim/door_states", rclcpp::SystemDefaultsQoS()); _door_request_sub = _ros_node->create_subscription( - ns + "/door_requests", rclcpp::SystemDefaultsQoS(), + "sim/door_requests", rclcpp::SystemDefaultsQoS(), [&](DoorRequest::UniquePtr msg) { if (msg->door_name == _state.door_name) diff --git a/rmf_building_sim_common/src/lift_common.cpp b/rmf_building_sim_common/src/lift_common.cpp index 2d55f30..6130a43 100644 --- a/rmf_building_sim_common/src/lift_common.cpp +++ b/rmf_building_sim_common/src/lift_common.cpp @@ -184,8 +184,7 @@ LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, std::string, std::vector> floor_name_to_cabin_door_name, std::unordered_map shaft_door_states, std::unordered_map cabin_door_states, - std::string initial_floor_name, - const std::string& lift_namespace) + std::string initial_floor_name) : _ros_node(node), _lift_name(lift_name), _cabin_joint_name(joint_name), @@ -203,19 +202,16 @@ LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, for (const auto& it : _floor_name_to_elevation) std::cout << it.first << " | " << it.second << std::endl; - std::string ns; - if (lift_namespace.size() > 0) - ns = "/" + lift_namespace; // initialize pub & sub _lift_state_pub = _ros_node->create_publisher( - ns + "/lift_states", rclcpp::SystemDefaultsQoS()); + "sim/lift_states", rclcpp::SystemDefaultsQoS()); // TODO(luca) remove adapter door request and make lift command its own doors _door_request_pub = _ros_node->create_publisher( "/adapter_door_requests", rclcpp::SystemDefaultsQoS()); _lift_request_sub = _ros_node->create_subscription( - ns + "/lift_requests", rclcpp::SystemDefaultsQoS(), + "sim/lift_requests", rclcpp::SystemDefaultsQoS(), [&](LiftRequest::UniquePtr msg) { if (msg->lift_name != _lift_name) From 19c4f841f9efdc171d0d0af747ed186ce48cc793 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 8 Dec 2022 12:57:49 +0800 Subject: [PATCH 4/4] Remove extra / Signed-off-by: Luca Della Vedova --- rmf_building_sim_common/src/lift_common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_building_sim_common/src/lift_common.cpp b/rmf_building_sim_common/src/lift_common.cpp index 6130a43..d9ca682 100644 --- a/rmf_building_sim_common/src/lift_common.cpp +++ b/rmf_building_sim_common/src/lift_common.cpp @@ -208,7 +208,7 @@ LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, // TODO(luca) remove adapter door request and make lift command its own doors _door_request_pub = _ros_node->create_publisher( - "/adapter_door_requests", rclcpp::SystemDefaultsQoS()); + "adapter_door_requests", rclcpp::SystemDefaultsQoS()); _lift_request_sub = _ros_node->create_subscription( "sim/lift_requests", rclcpp::SystemDefaultsQoS(),