From 4d917de4c6236ec6c668ee584438de70d48dd480 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 28 Jun 2024 20:20:04 +0200 Subject: [PATCH 1/7] Add forgotten remove_node calls --- tesseract_monitoring/src/environment_monitor.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tesseract_monitoring/src/environment_monitor.cpp b/tesseract_monitoring/src/environment_monitor.cpp index 4d673340..abe1913f 100644 --- a/tesseract_monitoring/src/environment_monitor.cpp +++ b/tesseract_monitoring/src/environment_monitor.cpp @@ -93,6 +93,7 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node internal_node_spinner_ = std::make_shared([this]() { internal_node_executor_->add_node(internal_node_); internal_node_executor_->spin(); + internal_node_executor_->remove_node(internal_node_); }); } @@ -113,6 +114,7 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node internal_node_spinner_ = std::make_shared([this]() { internal_node_executor_->add_node(internal_node_); internal_node_executor_->spin(); + internal_node_executor_->remove_node(internal_node_); }); } From 9d5b98f8656c83a3749fd05c216cd71afc069e37 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 28 Jun 2024 20:22:06 +0200 Subject: [PATCH 2/7] Use internal_node_ for all node calls (except parameters) and remove node_ member --- .../environment_monitor.h | 1 - .../src/environment_monitor.cpp | 30 +++++++++---------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/tesseract_monitoring/include/tesseract_monitoring/environment_monitor.h b/tesseract_monitoring/include/tesseract_monitoring/environment_monitor.h index 9d51188c..674e5ef4 100644 --- a/tesseract_monitoring/include/tesseract_monitoring/environment_monitor.h +++ b/tesseract_monitoring/include/tesseract_monitoring/environment_monitor.h @@ -143,7 +143,6 @@ class ROSEnvironmentMonitor : public tesseract_environment::EnvironmentMonitor rclcpp::Time last_robot_motion_time_ = rclcpp::Time(0L, RCL_ROS_TIME); /// Last time the robot has moved bool enforce_next_state_update_{ false }; /// flag to enforce immediate state update in onStateUpdate() - rclcpp::Node::SharedPtr node_; rclcpp::Node::SharedPtr internal_node_; rclcpp::executors::MultiThreadedExecutor::SharedPtr internal_node_executor_; std::shared_ptr internal_node_spinner_; diff --git a/tesseract_monitoring/src/environment_monitor.cpp b/tesseract_monitoring/src/environment_monitor.cpp index abe1913f..dff093de 100644 --- a/tesseract_monitoring/src/environment_monitor.cpp +++ b/tesseract_monitoring/src/environment_monitor.cpp @@ -59,7 +59,6 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node std::string robot_description, std::string monitor_namespace) : EnvironmentMonitor(std::move(monitor_namespace)) - , node_(node) , internal_node_(std::make_shared("ROSEnvironmentMonitor_internal", node->get_fully_qualified_name())) , robot_description_(std::move(robot_description)) , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant)) @@ -68,12 +67,12 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node // Initial setup std::string urdf_xml_string; std::string srdf_xml_string; - if (!node_->get_parameter(robot_description_, urdf_xml_string)) + if (!node->get_parameter(robot_description_, urdf_xml_string)) { RCLCPP_ERROR(logger_, "Failed to find parameter: %s", robot_description_.c_str()); return; } - if (!node_->get_parameter(robot_description_ + "_semantic", srdf_xml_string)) + if (!node->get_parameter(robot_description_ + "_semantic", srdf_xml_string)) { RCLCPP_ERROR(logger_, "Failed to find parameter: %s", (robot_description_ + "_semantic").c_str()); return; @@ -101,7 +100,6 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node tesseract_environment::Environment::Ptr env, std::string monitor_namespace) : EnvironmentMonitor(std::move(env), std::move(monitor_namespace)) - , node_(node) , internal_node_(std::make_shared("ROSEnvironmentMonitor_internal", node->get_fully_qualified_name())) , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant)) , logger_{ internal_node_->get_logger().get_child(monitor_namespace_ + "_monitor") } @@ -246,7 +244,7 @@ void ROSEnvironmentMonitor::sceneStateChangedCallback(const tesseract_environmen if (!monitored_environment_subscriber_ && !current_state_monitor_ && (typeid(event) == typeid(tesseract_environment::SceneStateChangedEvent))) { - last_update_time_ = last_robot_motion_time_ = node_->now(); + last_update_time_ = last_robot_motion_time_ = internal_node_->now(); } } @@ -278,7 +276,7 @@ void ROSEnvironmentMonitor::startPublishingEnvironment() std::string environment_topic = R"(/)" + monitor_namespace_ + DEFAULT_PUBLISH_ENVIRONMENT_TOPIC; environment_publisher_ = - node_->create_publisher(environment_topic, rclcpp::QoS(100)); + internal_node_->create_publisher(environment_topic, rclcpp::QoS(100)); RCLCPP_INFO(logger_, "Publishing maintained environment on '%s'", environment_topic.c_str()); publish_environment_ = std::make_unique([this]() { environmentPublishingThread(); }); } @@ -377,7 +375,7 @@ double ROSEnvironmentMonitor::getStateUpdateFrequency() const void ROSEnvironmentMonitor::newEnvironmentStateCallback( const tesseract_msgs::msg::EnvironmentState::ConstSharedPtr env) // NOLINT { - last_update_time_ = node_->now(); + last_update_time_ = internal_node_->now(); if (!env_->isInitialized()) { @@ -551,7 +549,7 @@ bool ROSEnvironmentMonitor::applyEnvironmentCommandsMessage( { if (tesseract_rosutils::processMsg(*env_, cmd.joint_state)) { - last_robot_motion_time_ = node_->now(); + last_robot_motion_time_ = internal_node_->now(); } else { @@ -584,7 +582,7 @@ bool ROSEnvironmentMonitor::waitForCurrentState(std::chrono::duration du if (std::chrono::duration_cast(duration).count() == 0) return false; - rclcpp::Time t = node_->now(); + rclcpp::Time t = internal_node_->now(); rclcpp::Time start = rclcpp::Clock().now(); auto timeout = rclcpp::Duration::from_seconds(duration.count()); @@ -745,8 +743,8 @@ void ROSEnvironmentMonitor::setStateUpdateFrequency(double hz) { std::scoped_lock lock(state_pending_mutex_); dt_state_update_ = rclcpp::Duration::from_seconds(1.0 / hz); - state_update_timer_ = node_->create_wall_timer(dt_state_update_.to_chrono>(), - [this]() { updateJointStateTimerCallback(); }); + state_update_timer_ = internal_node_->create_wall_timer(dt_state_update_.to_chrono>(), + [this]() { updateJointStateTimerCallback(); }); publish_ = true; } else @@ -769,11 +767,11 @@ void ROSEnvironmentMonitor::updateEnvironmentWithCurrentState() { std::vector missing; if (!current_state_monitor_->haveCompleteState(missing) && - (node_->now() - current_state_monitor_->getMonitorStartTime()).seconds() > 1.0) + (internal_node_->now() - current_state_monitor_->getMonitorStartTime()).seconds() > 1.0) { std::string missing_str = boost::algorithm::join(missing, ", "); RCLCPP_WARN_THROTTLE(logger_, - *node_->get_clock(), + *internal_node_->get_clock(), 1.0, "The complete state of the robot is not yet known. Missing %s", missing_str.c_str()); @@ -785,8 +783,10 @@ void ROSEnvironmentMonitor::updateEnvironmentWithCurrentState() env_->setState(current_state_monitor_->getCurrentState().joints); } else - RCLCPP_ERROR_THROTTLE( - logger_, *node_->get_clock(), 1.0, "State monitor is not active. Unable to set the planning scene state"); + RCLCPP_ERROR_THROTTLE(logger_, + *internal_node_->get_clock(), + 1.0, + "State monitor is not active. Unable to set the planning scene state"); } void ROSEnvironmentMonitor::setEnvironmentPublishingFrequency(double hz) From 076325c84b8c79cf821cbfe410c5ef426e2881aa Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 28 Jun 2024 20:56:12 +0200 Subject: [PATCH 3/7] Rename node_ to internal_node_ to be consistent with e.g. ROSEnvironmentMonitor --- tesseract_rosutils/src/plotting.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp index 9ad306a0..9c7bf5e5 100644 --- a/tesseract_rosutils/src/plotting.cpp +++ b/tesseract_rosutils/src/plotting.cpp @@ -65,9 +65,9 @@ ROSPlotting::ROSPlotting(std::string root_link, std::string topic_namespace) internal_node_executor_ = std::make_shared(); internal_node_spinner_ = std::make_shared([this]() { - internal_node_executor_->add_node(node_); + internal_node_executor_->add_node(internal_node_); internal_node_executor_->spin(); - internal_node_executor_->remove_node(node_); + internal_node_executor_->remove_node(internal_node_); }); } @@ -232,7 +232,8 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std: { const auto& m = dynamic_cast(marker); visualization_msgs::msg::MarkerArray msg; - auto arrow_marker_msg = getMarkerArrowMsg(marker_counter_, root_link_, topic_namespace_, node_->now(), m); + auto arrow_marker_msg = + getMarkerArrowMsg(marker_counter_, root_link_, topic_namespace_, internal_node_->now(), m); msg.markers.push_back(arrow_marker_msg); arrows_pub_->publish(msg); break; @@ -241,7 +242,7 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std: { const auto& m = dynamic_cast(marker); visualization_msgs::msg::MarkerArray msg = - getMarkerAxisMsg(marker_counter_, root_link_, topic_namespace_, node_->now(), m.axis, m.getScale()); + getMarkerAxisMsg(marker_counter_, root_link_, topic_namespace_, internal_node_->now(), m.axis, m.getScale()); axes_pub_->publish(msg); break; } @@ -254,7 +255,7 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std: visualization_msgs::msg::MarkerArray msg; long cnt = 0; - auto time = node_->now(); + auto time = internal_node_->now(); for (const auto& s : m.toolpath) { std::string segment_ns = prefix_ns + "/segment_" + std::to_string(cnt++) + "/poses"; @@ -274,7 +275,7 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std: if (!m.dist_results.empty()) { visualization_msgs::msg::MarkerArray msg = - getContactResultsMarkerArrayMsg(marker_counter_, root_link_, topic_namespace_, node_->now(), m); + getContactResultsMarkerArrayMsg(marker_counter_, root_link_, topic_namespace_, internal_node_->now(), m); collisions_pub_->publish(msg); } break; @@ -284,7 +285,7 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std: void ROSPlotting::plotMarkers(const std::vector& /*markers*/, std::string /*ns*/) { - RCLCPP_ERROR(node_->get_logger(), "ROSPlotting: Plotting vector of markers is currently not implemented!"); + RCLCPP_ERROR(internal_node_->get_logger(), "ROSPlotting: Plotting vector of markers is currently not implemented!"); } void ROSPlotting::plotToolpath(const tesseract_environment::Environment& env, From 615365c8f9eda8dcbbb094236f981a37bf9733e6 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 1 Jul 2024 16:39:16 +0200 Subject: [PATCH 4/7] Change callback group to MutuallyExclusive --- tesseract_monitoring/src/environment_monitor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_monitoring/src/environment_monitor.cpp b/tesseract_monitoring/src/environment_monitor.cpp index dff093de..e621c8ce 100644 --- a/tesseract_monitoring/src/environment_monitor.cpp +++ b/tesseract_monitoring/src/environment_monitor.cpp @@ -61,7 +61,7 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node : EnvironmentMonitor(std::move(monitor_namespace)) , internal_node_(std::make_shared("ROSEnvironmentMonitor_internal", node->get_fully_qualified_name())) , robot_description_(std::move(robot_description)) - , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant)) + , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)) , logger_{ internal_node_->get_logger().get_child(monitor_namespace_ + "_monitor") } { // Initial setup @@ -101,7 +101,7 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node std::string monitor_namespace) : EnvironmentMonitor(std::move(env), std::move(monitor_namespace)) , internal_node_(std::make_shared("ROSEnvironmentMonitor_internal", node->get_fully_qualified_name())) - , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant)) + , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)) , logger_{ internal_node_->get_logger().get_child(monitor_namespace_ + "_monitor") } { if (!initialize()) From 9ad59ce2b419fc717a863775d12c3a413d6340bb Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 3 Jul 2024 09:09:41 +0200 Subject: [PATCH 5/7] Fixup --- .../src/environment_monitor.cpp | 4 +-- .../include/tesseract_rosutils/plotting.h | 2 +- tesseract_rosutils/src/plotting.cpp | 26 ++++++++++++------- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/tesseract_monitoring/src/environment_monitor.cpp b/tesseract_monitoring/src/environment_monitor.cpp index e621c8ce..17c20def 100644 --- a/tesseract_monitoring/src/environment_monitor.cpp +++ b/tesseract_monitoring/src/environment_monitor.cpp @@ -187,7 +187,7 @@ bool ROSEnvironmentMonitor::initialize() return false; } - last_update_time_ = last_robot_motion_time_ = node_->now(); + last_update_time_ = last_robot_motion_time_ = internal_node_->now(); last_robot_state_update_wall_time_ = rclcpp::Clock().now(); state_update_pending_ = false; @@ -642,7 +642,7 @@ void ROSEnvironmentMonitor::startStateMonitor(const std::string& joint_states_to if (env_) { if (!current_state_monitor_) - current_state_monitor_ = std::make_unique(env_, node_); + current_state_monitor_ = std::make_unique(env_, internal_node_); current_state_monitor_->addUpdateCallback( std::bind(&ROSEnvironmentMonitor::onJointStateUpdate, this, std::placeholders::_1)); // NOLINT diff --git a/tesseract_rosutils/include/tesseract_rosutils/plotting.h b/tesseract_rosutils/include/tesseract_rosutils/plotting.h index e1c371ca..24ee3570 100644 --- a/tesseract_rosutils/include/tesseract_rosutils/plotting.h +++ b/tesseract_rosutils/include/tesseract_rosutils/plotting.h @@ -135,7 +135,7 @@ class ROSPlotting : public tesseract_visualization::Visualization std::string root_link_; /**< Root link of markers */ std::string topic_namespace_; /**< Namespace used when publishing markers */ int marker_counter_{ 0 }; /**< Counter when plotting */ - rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr internal_node_; rclcpp::executors::MultiThreadedExecutor::SharedPtr internal_node_executor_; std::shared_ptr internal_node_spinner_; rclcpp::Publisher::SharedPtr scene_pub_; /**< Scene publisher */ diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp index 9c7bf5e5..d4b03a42 100644 --- a/tesseract_rosutils/src/plotting.cpp +++ b/tesseract_rosutils/src/plotting.cpp @@ -53,15 +53,23 @@ static constexpr const char* NODE_ID = "tesseract_rosutils_plotting"; ROSPlotting::ROSPlotting(std::string root_link, std::string topic_namespace) : root_link_(std::move(root_link)), topic_namespace_(std::move(topic_namespace)) { - node_ = std::make_shared(NODE_ID); - trajectory_pub_ = - node_->create_publisher(topic_namespace_ + "/display_tesseract_trajectory", 1); - collisions_pub_ = - node_->create_publisher(topic_namespace_ + "/display_collisions", 1); - arrows_pub_ = node_->create_publisher(topic_namespace_ + "/display_arrows", 1); - axes_pub_ = node_->create_publisher(topic_namespace_ + "/display_axes", 1); - tool_path_pub_ = - node_->create_publisher(topic_namespace_ + "/display_tool_path", 1); + internal_node_ = std::make_shared(NODE_ID); + trajectory_pub_ = internal_node_->create_publisher(topic_namespace_ + "/display_" + "tesseract_" + "trajectory", + 1); + collisions_pub_ = internal_node_->create_publisher(topic_namespace_ + "/display" + "_collisi" + "ons", + 1); + arrows_pub_ = + internal_node_->create_publisher(topic_namespace_ + "/display_arrows", 1); + axes_pub_ = + internal_node_->create_publisher(topic_namespace_ + "/display_axes", 1); + tool_path_pub_ = internal_node_->create_publisher(topic_namespace_ + "/display_" + "tool_" + "path", + 1); internal_node_executor_ = std::make_shared(); internal_node_spinner_ = std::make_shared([this]() { From c0509e63977fef60fd9ee6bcc8939a810356285c Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jul 2024 07:30:26 +0200 Subject: [PATCH 6/7] Change ROSPlotting to use a SingleThreadedExecutor --- tesseract_rosutils/include/tesseract_rosutils/plotting.h | 2 +- tesseract_rosutils/src/plotting.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_rosutils/include/tesseract_rosutils/plotting.h b/tesseract_rosutils/include/tesseract_rosutils/plotting.h index 24ee3570..5d5500ee 100644 --- a/tesseract_rosutils/include/tesseract_rosutils/plotting.h +++ b/tesseract_rosutils/include/tesseract_rosutils/plotting.h @@ -136,7 +136,7 @@ class ROSPlotting : public tesseract_visualization::Visualization std::string topic_namespace_; /**< Namespace used when publishing markers */ int marker_counter_{ 0 }; /**< Counter when plotting */ rclcpp::Node::SharedPtr internal_node_; - rclcpp::executors::MultiThreadedExecutor::SharedPtr internal_node_executor_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr internal_node_executor_; std::shared_ptr internal_node_spinner_; rclcpp::Publisher::SharedPtr scene_pub_; /**< Scene publisher */ rclcpp::Publisher::SharedPtr trajectory_pub_; /**< Trajectory publisher */ diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp index d4b03a42..7c88e7a5 100644 --- a/tesseract_rosutils/src/plotting.cpp +++ b/tesseract_rosutils/src/plotting.cpp @@ -71,7 +71,7 @@ ROSPlotting::ROSPlotting(std::string root_link, std::string topic_namespace) "path", 1); - internal_node_executor_ = std::make_shared(); + internal_node_executor_ = std::make_shared(); internal_node_spinner_ = std::make_shared([this]() { internal_node_executor_->add_node(internal_node_); internal_node_executor_->spin(); From 0c9af8d5644f35b26ba09c45fbef8eef1d64ae73 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 12 Jul 2024 19:44:53 +0200 Subject: [PATCH 7/7] Change DEFAULT_JOINT_STATES_TOPIC to start with / --- tesseract_monitoring/include/tesseract_monitoring/constants.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_monitoring/include/tesseract_monitoring/constants.h b/tesseract_monitoring/include/tesseract_monitoring/constants.h index c3baec81..8413a04c 100644 --- a/tesseract_monitoring/include/tesseract_monitoring/constants.h +++ b/tesseract_monitoring/include/tesseract_monitoring/constants.h @@ -30,7 +30,7 @@ namespace tesseract_monitoring { /// The name of the topic used by default for receiving joint states -static const std::string DEFAULT_JOINT_STATES_TOPIC = "joint_states"; +static const std::string DEFAULT_JOINT_STATES_TOPIC = "/joint_states"; /// The name of the service used by default for requesting tesseract environment change history static const std::string DEFAULT_GET_ENVIRONMENT_CHANGES_SERVICE = "/get_tesseract_changes";