From 55181095d1438bc454a9f20d9b33ed22a86cf490 Mon Sep 17 00:00:00 2001 From: Abdelrahman Ehab Date: Mon, 19 Sep 2022 18:01:26 +0200 Subject: [PATCH 1/2] Reordered code execution MoveGroupInterface was moved before spinning up the executor. Reordered code execution JointModelGroup was moved before spinning up the executor. Also explained what the executor does. Reordered code execution JointModelGroup was moved before spinning up the executor. Also explained what the executor does. Fix minor build error, formatting --- .../src/motion_planning_api_tutorial.cpp | 10 ++++++---- .../src/motion_planning_pipeline_tutorial.cpp | 10 ++++++---- .../src/move_group_interface_tutorial.cpp | 14 +++++++------- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp b/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp index 498056314a..316b5a1464 100644 --- a/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp +++ b/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp @@ -56,10 +56,6 @@ int main(int argc, char** argv) std::shared_ptr motion_planning_api_tutorial_node = rclcpp::Node::make_shared("motion_planning_api_tutorial", node_options); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(motion_planning_api_tutorial_node); - std::thread([&executor]() { executor.spin(); }).detach(); - // BEGIN_TUTORIAL // Start // ^^^^^ @@ -79,6 +75,12 @@ int main(int argc, char** argv) /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); + + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(motion_planning_api_tutorial_node); + std::thread([&executor]() { executor.spin(); }).detach(); // Using the // :moveit_codedir:`RobotModel`, diff --git a/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp b/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp index 0a5706f48a..e8501249bd 100644 --- a/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp +++ b/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp @@ -56,10 +56,6 @@ int main(int argc, char** argv) node_options.automatically_declare_parameters_from_overrides(true); auto node = rclcpp::Node::make_shared("motion_planning_pipeline_tutorial", node_options); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - std::thread([&executor]() { executor.spin(); }).detach(); - // BEGIN_TUTORIAL // Start // ^^^^^ @@ -105,6 +101,12 @@ int main(int argc, char** argv) group is useful for dealing with one set of joints at a time such as a left arm or a end effector */ const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup("panda_arm"); + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + // We can now setup the PlanningPipeline object, which will use the ROS parameter server // to determine the set of request adapters and the planning plugin to use planning_pipeline::PlanningPipelinePtr planning_pipeline( diff --git a/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp index f3bb4951e1..53131544ae 100644 --- a/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp @@ -56,13 +56,7 @@ int main(int argc, char** argv) rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options); - - // We spin up a SingleThreadedExecutor for the current state monitor to get information - // about the robot's state. - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(move_group_node); - std::thread([&executor]() { executor.spin(); }).detach(); - + // BEGIN_TUTORIAL // // Setup @@ -77,6 +71,12 @@ int main(int argc, char** argv) // :moveit_codedir:`MoveGroupInterface` // class can be easily set up using just the name of the planning group you would like to control and plan for. moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); + + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); // We will use the // :moveit_codedir:`PlanningSceneInterface` From 191404697fe8bb9ab05b3efedd348c5a97e2c9e0 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 19 Sep 2022 16:21:08 -0500 Subject: [PATCH 2/2] Whitespace formatting --- .../motion_planning_api/src/motion_planning_api_tutorial.cpp | 2 +- .../src/move_group_interface_tutorial.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp b/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp index 316b5a1464..9ba519d61d 100644 --- a/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp +++ b/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp @@ -75,7 +75,7 @@ int main(int argc, char** argv) /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/ moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); - + // We spin up a SingleThreadedExecutor for the current state monitor to get information // about the robot's state. rclcpp::executors::SingleThreadedExecutor executor; diff --git a/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp index 53131544ae..e8bf1ba1d7 100644 --- a/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp @@ -56,7 +56,7 @@ int main(int argc, char** argv) rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options); - + // BEGIN_TUTORIAL // // Setup @@ -71,7 +71,7 @@ int main(int argc, char** argv) // :moveit_codedir:`MoveGroupInterface` // class can be easily set up using just the name of the planning group you would like to control and plan for. moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); - + // We spin up a SingleThreadedExecutor for the current state monitor to get information // about the robot's state. rclcpp::executors::SingleThreadedExecutor executor;