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"; 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 4d673340..17c20def 100644 --- a/tesseract_monitoring/src/environment_monitor.cpp +++ b/tesseract_monitoring/src/environment_monitor.cpp @@ -59,21 +59,20 @@ 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)) + , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)) , logger_{ internal_node_->get_logger().get_child(monitor_namespace_ + "_monitor") } { // 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; @@ -93,6 +92,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_); }); } @@ -100,9 +100,8 @@ 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)) + , cb_group_(internal_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)) , logger_{ internal_node_->get_logger().get_child(monitor_namespace_ + "_monitor") } { if (!initialize()) @@ -113,6 +112,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_); }); } @@ -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; @@ -244,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(); } } @@ -276,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(); }); } @@ -375,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()) { @@ -549,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 { @@ -582,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()); @@ -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 @@ -743,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 @@ -767,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()); @@ -783,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) diff --git a/tesseract_rosutils/include/tesseract_rosutils/plotting.h b/tesseract_rosutils/include/tesseract_rosutils/plotting.h index e1c371ca..5d5500ee 100644 --- a/tesseract_rosutils/include/tesseract_rosutils/plotting.h +++ b/tesseract_rosutils/include/tesseract_rosutils/plotting.h @@ -135,8 +135,8 @@ 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::executors::MultiThreadedExecutor::SharedPtr internal_node_executor_; + rclcpp::Node::SharedPtr internal_node_; + 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 9ad306a0..7c88e7a5 100644 --- a/tesseract_rosutils/src/plotting.cpp +++ b/tesseract_rosutils/src/plotting.cpp @@ -53,21 +53,29 @@ 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_executor_ = std::make_shared(); + 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]() { - 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 +240,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 +250,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 +263,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 +283,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 +293,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,