Skip to content

Commit

Permalink
initializes all ros interfaces with the private node
Browse files Browse the repository at this point in the history
  • Loading branch information
@jrgnicho
jrgnicho committed Aug 12, 2020
1 parent cc6bb8a commit 43e41c0
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 17 deletions.
1 change: 1 addition & 0 deletions ...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -461,6 +461,7 @@ class PlanningSceneMonitor : private boost::noncopyable
// TODO: (anasarrak) callbacks on ROS2?
// https://answers.ros.org/question/300874/how-do-you-use-callbackgroups-as-a-replacement-for-callbackqueues-in-ros2/
// ros::CallbackQueue queue_;
std::shared_ptr<rclcpp::Node> pnode_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> private_executor_;

std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
Expand Down
33 changes: 16 additions & 17 deletions moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,7 @@ PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node,
const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
: monitor_name_(name)
, node_(node)
, pnode_(std::make_shared<rclcpp::Node>(std::string(node_->get_fully_qualified_name()) + "_private"))
, private_executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>())
, tf_buffer_(tf_buffer)
, dt_state_update_(0.0)
Expand Down Expand Up @@ -200,14 +201,14 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
{
robot_model_ = rm_loader_->getModel();
scene_ = scene;
collision_loader_.setupScene(node_, scene_);
collision_loader_.setupScene(pnode_, scene_);
scene_const_ = scene_;
if (!scene_)
{
try
{
scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
collision_loader_.setupScene(node_, scene_);
collision_loader_.setupScene(pnode_, scene_);
scene_const_ = scene_;
configureCollisionMatrix(scene_);
configureDefaultPadding();
Expand Down Expand Up @@ -262,11 +263,9 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
state_update_pending_ = false;
// Period for 0.1 sec
using std::chrono::nanoseconds;
std::shared_ptr<rclcpp::Node> pnode =
std::make_shared<rclcpp::Node>(std::string(node_->get_fully_qualified_name()) + "_private");
state_update_timer_ =
pnode->create_wall_timer(dt_state_update_, std::bind(&PlanningSceneMonitor::stateUpdateTimerCallback, this));
private_executor_->add_node(pnode);
pnode_->create_wall_timer(dt_state_update_, std::bind(&PlanningSceneMonitor::stateUpdateTimerCallback, this));
private_executor_->add_node(pnode_);

// start executor on a different thread now
std::thread([this]() { private_executor_->spin(); }).detach();
Expand Down Expand Up @@ -340,7 +339,7 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t
publish_update_types_ = update_type;
if (!publish_planning_scene_ && scene_)
{
planning_scene_publisher_ = node_->create_publisher<moveit_msgs::msg::PlanningScene>(planning_scene_topic, 100);
planning_scene_publisher_ = pnode_->create_publisher<moveit_msgs::msg::PlanningScene>(planning_scene_topic, 100);
RCLCPP_INFO(LOGGER, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
monitorDiffs(true);
publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
Expand Down Expand Up @@ -496,7 +495,7 @@ bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_
throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name);
}
// use global namespace for service
auto client = node_->create_client<moveit_msgs::srv::GetPlanningScene>(service_name);
auto client = pnode_->create_client<moveit_msgs::srv::GetPlanningScene>(service_name);
auto srv = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
srv->components.components = srv->components.SCENE_SETTINGS | srv->components.ROBOT_STATE |
srv->components.ROBOT_STATE_ATTACHED_OBJECTS | srv->components.WORLD_OBJECT_NAMES |
Expand Down Expand Up @@ -536,7 +535,7 @@ bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_
void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name)
{
// Load the service
get_scene_service_ = node_->create_service<moveit_msgs::srv::GetPlanningScene>(
get_scene_service_ = pnode_->create_service<moveit_msgs::srv::GetPlanningScene>(
service_name, std::bind(&PlanningSceneMonitor::getPlanningSceneServiceCallback, this, std::placeholders::_1,
std::placeholders::_2));
}
Expand Down Expand Up @@ -1007,7 +1006,7 @@ void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
// listen for planning scene updates; these messages include transforms, so no need for filters
if (!scene_topic.empty())
{
planning_scene_subscriber_ = node_->create_subscription<moveit_msgs::msg::PlanningScene>(
planning_scene_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::PlanningScene>(
scene_topic, 100, std::bind(&PlanningSceneMonitor::newPlanningSceneCallback, this, std::placeholders::_1));
RCLCPP_INFO(LOGGER, "Listening to '%s'", planning_scene_subscriber_->get_topic_name());
}
Expand Down Expand Up @@ -1089,15 +1088,15 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio
// Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
if (!collision_objects_topic.empty())
{
collision_object_subscriber_ = node_->create_subscription<moveit_msgs::msg::CollisionObject>(
collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
collision_objects_topic, 1024,
std::bind(&PlanningSceneMonitor::collisionObjectCallback, this, std::placeholders::_1));
RCLCPP_INFO(LOGGER, "Listening to '%s'", collision_objects_topic.c_str());
}

if (!planning_scene_world_topic.empty())
{
planning_scene_world_subscriber_ = node_->create_subscription<moveit_msgs::msg::PlanningSceneWorld>(
planning_scene_world_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::PlanningSceneWorld>(
planning_scene_world_topic, 1,
std::bind(&PlanningSceneMonitor::newPlanningSceneWorldCallback, this, std::placeholders::_1));
RCLCPP_INFO(LOGGER, "Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str());
Expand All @@ -1109,7 +1108,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio
if (!octomap_monitor_)
{
octomap_monitor_.reset(
new occupancy_map_monitor::OccupancyMapMonitor(node_, tf_buffer_, scene_->getPlanningFrame()));
new occupancy_map_monitor::OccupancyMapMonitor(pnode_, tf_buffer_, scene_->getPlanningFrame()));
excludeRobotLinksFromOctree();
excludeAttachedBodiesFromOctree();
excludeWorldObjectsFromOctree();
Expand Down Expand Up @@ -1145,7 +1144,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top
if (scene_)
{
if (!current_state_monitor_)
current_state_monitor_.reset(new CurrentStateMonitor(node_, getRobotModel(), tf_buffer_));
current_state_monitor_.reset(new CurrentStateMonitor(pnode_, getRobotModel(), tf_buffer_));
current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
current_state_monitor_->startStateMonitor(joint_states_topic);

Expand All @@ -1154,14 +1153,14 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top
if (dt_state_update_.count() > 0)
// ROS original: state_update_timer_.start();
// TODO: re-enable WallTimer start()
state_update_timer_ = node_->create_wall_timer(
state_update_timer_ = pnode_->create_wall_timer(
dt_state_update_, std::bind(&PlanningSceneMonitor::stateUpdateTimerCallback, this));
}

if (!attached_objects_topic.empty())
{
// using regular message filter as there's no header
attached_collision_object_subscriber_ = node_->create_subscription<moveit_msgs::msg::AttachedCollisionObject>(
attached_collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::AttachedCollisionObject>(
attached_objects_topic, 1024,
std::bind(&PlanningSceneMonitor::attachObjectCallback, this, std::placeholders::_1));
RCLCPP_INFO(LOGGER, "Listening to '%s' for attached collision objects",
Expand Down Expand Up @@ -1277,7 +1276,7 @@ void PlanningSceneMonitor::setStateUpdateFrequency(double hz)
// ROS original: state_update_timer_.start();
// TODO: re-enable WallTimer start()
state_update_timer_ =
node_->create_wall_timer(dt_state_update_, std::bind(&PlanningSceneMonitor::stateUpdateTimerCallback, this));
pnode_->create_wall_timer(dt_state_update_, std::bind(&PlanningSceneMonitor::stateUpdateTimerCallback, this));
}
else
{
Expand Down

0 comments on commit 43e41c0

Please sign in to comment.