Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Node cleanup #109

Merged
merged 7 commits into from
Jul 12, 2024
Merged

Node cleanup #109

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
4d917de
Add forgotten remove_node calls
rjoomen Jun 28, 2024
9d5b98f
Use internal_node_ for all node calls (except parameters) and remove …
rjoomen Jun 28, 2024
076325c
Rename node_ to internal_node_ to be consistent with e.g. ROSEnvironm…
rjoomen Jun 28, 2024
615365c
Change callback group to MutuallyExclusive
rjoomen Jul 1, 2024
9ad59ce
Fixup
rjoomen Jul 3, 2024
c0509e6
Change ROSPlotting to use a SingleThreadedExecutor
rjoomen Jul 9, 2024
0c9af8d
Change DEFAULT_JOINT_STATES_TOPIC to start with /
rjoomen Jul 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion tesseract_monitoring/include/tesseract_monitoring/constants.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading