Skip to content

Commit

Permalink
Node cleanup (#109)
Browse files Browse the repository at this point in the history 
* Add forgotten remove_node calls

* Use internal_node_ for all node calls (except parameters) and remove node_ member

* Rename node_ to internal_node_ to be consistent with e.g. ROSEnvironmentMonitor

* Change callback group to MutuallyExclusive

* Fixup

* Change ROSPlotting to use a SingleThreadedExecutor

* Change DEFAULT_JOINT_STATES_TOPIC to start with /
  • Loading branch information
@rjoomen
rjoomen authored Jul 12, 2024
1 parent 297c64e commit 9196308
Show file tree
Hide file tree
Showing 5 changed files with 51 additions and 41 deletions.
2 changes: 1 addition & 1 deletion tesseract_monitoring/include/tesseract_monitoring/constants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down
1 change: 0 additions & 1 deletion tesseract_monitoring/include/tesseract_monitoring/environment_monitor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::thread> internal_node_spinner_;
Expand Down
40 changes: 21 additions & 19 deletions tesseract_monitoring/src/environment_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<rclcpp::Node>("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;
Expand All @@ -93,16 +92,16 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node
internal_node_spinner_ = std::make_shared<std::thread>([this]() {
internal_node_executor_->add_node(internal_node_);
internal_node_executor_->spin();
internal_node_executor_->remove_node(internal_node_);
});
}

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<rclcpp::Node>("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())
Expand All @@ -113,6 +112,7 @@ ROSEnvironmentMonitor::ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node
internal_node_spinner_ = std::make_shared<std::thread>([this]() {
internal_node_executor_->add_node(internal_node_);
internal_node_executor_->spin();
internal_node_executor_->remove_node(internal_node_);
});
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();
}
}

Expand Down Expand Up @@ -276,7 +276,7 @@ void ROSEnvironmentMonitor::startPublishingEnvironment()

std::string environment_topic = R"(/)" + monitor_namespace_ + DEFAULT_PUBLISH_ENVIRONMENT_TOPIC;
environment_publisher_ =
node_->create_publisher<tesseract_msgs::msg::EnvironmentState>(environment_topic, rclcpp::QoS(100));
internal_node_->create_publisher<tesseract_msgs::msg::EnvironmentState>(environment_topic, rclcpp::QoS(100));
RCLCPP_INFO(logger_, "Publishing maintained environment on '%s'", environment_topic.c_str());
publish_environment_ = std::make_unique<std::thread>([this]() { environmentPublishingThread(); });
}
Expand Down Expand Up @@ -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())
{
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -582,7 +582,7 @@ bool ROSEnvironmentMonitor::waitForCurrentState(std::chrono::duration<double> du
if (std::chrono::duration_cast<std::chrono::seconds>(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());

Expand Down Expand Up @@ -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<CurrentStateMonitor>(env_, node_);
current_state_monitor_ = std::make_unique<CurrentStateMonitor>(env_, internal_node_);

current_state_monitor_->addUpdateCallback(
std::bind(&ROSEnvironmentMonitor::onJointStateUpdate, this, std::placeholders::_1)); // NOLINT
Expand Down Expand Up @@ -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<std::chrono::duration<double>>(),
[this]() { updateJointStateTimerCallback(); });
state_update_timer_ = internal_node_->create_wall_timer(dt_state_update_.to_chrono<std::chrono::duration<double>>(),
[this]() { updateJointStateTimerCallback(); });
publish_ = true;
}
else
Expand All @@ -767,11 +767,11 @@ void ROSEnvironmentMonitor::updateEnvironmentWithCurrentState()
{
std::vector<std::string> 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());
Expand All @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions tesseract_rosutils/include/tesseract_rosutils/plotting.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::thread> internal_node_spinner_;
rclcpp::Publisher<tesseract_msgs::msg::SceneGraph>::SharedPtr scene_pub_; /**< Scene publisher */
rclcpp::Publisher<tesseract_msgs::msg::Trajectory>::SharedPtr trajectory_pub_; /**< Trajectory publisher */
Expand Down
45 changes: 27 additions & 18 deletions tesseract_rosutils/src/plotting.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<rclcpp::Node>(NODE_ID);
trajectory_pub_ =
node_->create_publisher<tesseract_msgs::msg::Trajectory>(topic_namespace_ + "/display_tesseract_trajectory", 1);
collisions_pub_ =
node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display_collisions", 1);
arrows_pub_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display_arrows", 1);
axes_pub_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display_axes", 1);
tool_path_pub_ =
node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display_tool_path", 1);

internal_node_executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
internal_node_ = std::make_shared<rclcpp::Node>(NODE_ID);
trajectory_pub_ = internal_node_->create_publisher<tesseract_msgs::msg::Trajectory>(topic_namespace_ + "/display_"
"tesseract_"
"trajectory",
1);
collisions_pub_ = internal_node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display"
"_collisi"
"ons",
1);
arrows_pub_ =
internal_node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display_arrows", 1);
axes_pub_ =
internal_node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display_axes", 1);
tool_path_pub_ = internal_node_->create_publisher<visualization_msgs::msg::MarkerArray>(topic_namespace_ + "/display_"
"tool_"
"path",
1);

internal_node_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
internal_node_spinner_ = std::make_shared<std::thread>([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_);
});
}

Expand Down Expand Up @@ -232,7 +240,8 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std:
{
const auto& m = dynamic_cast<const tesseract_visualization::ArrowMarker&>(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;
Expand All @@ -241,7 +250,7 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std:
{
const auto& m = dynamic_cast<const tesseract_visualization::AxisMarker&>(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;
}
Expand All @@ -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";
Expand All @@ -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;
Expand All @@ -284,7 +293,7 @@ void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std:

void ROSPlotting::plotMarkers(const std::vector<tesseract_visualization::Marker::Ptr>& /*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,
Expand Down

0 comments on commit 9196308

Please sign in to comment.