mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Simplify rclcpp::Node creation
Directly pass node name and namespace instead of using options.
This commit is contained in:
parent
5ae22da8db
commit
046309d60a
@ -108,9 +108,7 @@ class IntrospectionPrivate
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : task_(task), task_id_(getTaskId(task)) {
|
IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : task_(task), task_id_(getTaskId(task)) {
|
||||||
rclcpp::NodeOptions options;
|
node_ = rclcpp::Node::make_shared("introspection_" + task_id_, task_->ns());
|
||||||
options.arguments({ "--ros-args", "-r", "__node:=introspection_" + task_id_ });
|
|
||||||
node_ = rclcpp::Node::make_shared("_", task_->ns(), options);
|
|
||||||
executor_.add_node(node_);
|
executor_.add_node(node_);
|
||||||
task_description_publisher_ = node_->create_publisher<moveit_task_constructor_msgs::msg::TaskDescription>(
|
task_description_publisher_ = node_->create_publisher<moveit_task_constructor_msgs::msg::TaskDescription>(
|
||||||
DESCRIPTION_TOPIC, rclcpp::QoS(2).transient_local());
|
DESCRIPTION_TOPIC, rclcpp::QoS(2).transient_local());
|
||||||
|
|||||||
@ -75,10 +75,7 @@ void CurrentState::compute() {
|
|||||||
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
|
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
|
||||||
|
|
||||||
// Add random ID to prevent warnings about multiple publishers within the same node
|
// Add random ID to prevent warnings about multiple publishers within the same node
|
||||||
rclcpp::NodeOptions options;
|
auto node = rclcpp::Node::make_shared("current_state_" + std::to_string(reinterpret_cast<std::size_t>(this)));
|
||||||
options.arguments(
|
|
||||||
{ "--ros-args", "-r", "__node:=current_state_" + std::to_string(reinterpret_cast<std::size_t>(this)) });
|
|
||||||
auto node = rclcpp::Node::make_shared("_", options);
|
|
||||||
auto client = node->create_client<moveit_msgs::srv::GetPlanningScene>("get_planning_scene");
|
auto client = node->create_client<moveit_msgs::srv::GetPlanningScene>("get_planning_scene");
|
||||||
|
|
||||||
auto timeout = std::chrono::duration<double>(this->timeout());
|
auto timeout = std::chrono::duration<double>(this->timeout());
|
||||||
|
|||||||
@ -270,11 +270,8 @@ void Task::preempt() {
|
|||||||
|
|
||||||
moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
|
moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
|
||||||
// Add random ID to prevent warnings about multiple publishers within the same node
|
// Add random ID to prevent warnings about multiple publishers within the same node
|
||||||
rclcpp::NodeOptions options;
|
auto node = rclcpp::Node::make_shared("moveit_task_constructor_executor_" +
|
||||||
options.arguments(
|
std::to_string(reinterpret_cast<std::size_t>(this)));
|
||||||
{ "--ros-args", "-r",
|
|
||||||
"__node:=moveit_task_constructor_executor_" + std::to_string(reinterpret_cast<std::size_t>(this)) });
|
|
||||||
auto node = rclcpp::Node::make_shared("_", options);
|
|
||||||
auto ac = rclcpp_action::create_client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
auto ac = rclcpp_action::create_client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
||||||
node, "execute_task_solution");
|
node, "execute_task_solution");
|
||||||
if (!ac->wait_for_action_server(0.5s)) {
|
if (!ac->wait_for_action_server(0.5s)) {
|
||||||
|
|||||||
@ -189,11 +189,8 @@ RemoteTaskModel::RemoteTaskModel(const std::string& service_name, const planning
|
|||||||
: BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) {
|
: BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) {
|
||||||
id_to_stage_[0] = root_; // root node has ID 0
|
id_to_stage_[0] = root_; // root node has ID 0
|
||||||
// Add random ID to prevent warnings about multiple publishers within the same node
|
// Add random ID to prevent warnings about multiple publishers within the same node
|
||||||
rclcpp::NodeOptions options;
|
node_ = rclcpp::Node::make_shared("get_solution_node_" + std::to_string(reinterpret_cast<std::size_t>(this)),
|
||||||
options.arguments({ "--ros-args", "-r",
|
"/moveit_task_constructor/remote_task_model");
|
||||||
"__node:=get_solution_node_" + std::to_string(reinterpret_cast<std::size_t>(this)), "-r",
|
|
||||||
"__ns:=/moveit_task_constructor/remote_task_model" });
|
|
||||||
node_ = rclcpp::Node::make_shared("_", options);
|
|
||||||
// service to request solutions
|
// service to request solutions
|
||||||
get_solution_client_ = node_->create_client<moveit_task_constructor_msgs::srv::GetSolution>(service_name);
|
get_solution_client_ = node_->create_client<moveit_task_constructor_msgs::srv::GetSolution>(service_name);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -228,9 +228,7 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep
|
|||||||
TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view) {
|
TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view) {
|
||||||
setupUi(view);
|
setupUi(view);
|
||||||
|
|
||||||
rclcpp::NodeOptions options;
|
node_ = rclcpp::Node::make_shared("task_view_private", "");
|
||||||
options.arguments({ "--ros-args", "-r", "__node:=task_view_private" });
|
|
||||||
node_ = rclcpp::Node::make_shared("_", "", options);
|
|
||||||
exec_action_client_ = rclcpp_action::create_client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
exec_action_client_ = rclcpp_action::create_client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
||||||
node_, "execute_task_solution");
|
node_, "execute_task_solution");
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user