From 7f9e38ffdc832ba3aaca20d5833c37e7a4f33640 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 15 Apr 2021 10:44:40 +0000 Subject: [PATCH 1/7] Port planning_scene_tutorial --- CMakeLists.txt | 2 +- doc/planning_scene/CMakeLists.txt | 16 +++- .../launch/planning_scene_tutorial.launch | 10 --- .../launch/planning_scene_tutorial.launch.py | 56 ++++++++++++ .../planning_scene_tutorial.rst | 39 ++++----- .../src/planning_scene_tutorial.cpp | 86 ++++++------------- 6 files changed, 114 insertions(+), 95 deletions(-) delete mode 100644 doc/planning_scene/launch/planning_scene_tutorial.launch create mode 100644 doc/planning_scene/launch/planning_scene_tutorial.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 1bb11f1726..787d530747 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,7 +60,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) # add_subdirectory(doc/perception_pipeline) # add_subdirectory(doc/pick_place) # add_subdirectory(doc/planning) -# add_subdirectory(doc/planning_scene) +add_subdirectory(doc/planning_scene) # add_subdirectory(doc/planning_scene_ros_api) add_subdirectory(doc/quickstart_in_rviz) # add_subdirectory(doc/robot_model_and_robot_state) diff --git a/doc/planning_scene/CMakeLists.txt b/doc/planning_scene/CMakeLists.txt index 388a84c7d7..5d58961926 100644 --- a/doc/planning_scene/CMakeLists.txt +++ b/doc/planning_scene/CMakeLists.txt @@ -1,5 +1,13 @@ -add_executable(planning_scene_tutorial src/planning_scene_tutorial.cpp) -target_link_libraries(planning_scene_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS planning_scene_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +add_executable(planning_scene_tutorial + src/planning_scene_tutorial.cpp) +target_include_directories(planning_scene_tutorial + PUBLIC include) +ament_target_dependencies(planning_scene_tutorial + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS planning_scene_tutorial + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) \ No newline at end of file diff --git a/doc/planning_scene/launch/planning_scene_tutorial.launch b/doc/planning_scene/launch/planning_scene_tutorial.launch deleted file mode 100644 index 9e198a57f3..0000000000 --- a/doc/planning_scene/launch/planning_scene_tutorial.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/doc/planning_scene/launch/planning_scene_tutorial.launch.py b/doc/planning_scene/launch/planning_scene_tutorial.launch.py new file mode 100644 index 0000000000..1b5dc8b21a --- /dev/null +++ b/doc/planning_scene/launch/planning_scene_tutorial.launch.py @@ -0,0 +1,56 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # planning_context + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + # Planning Scene Tutorial executable + planning_scene_tutorial = Node( + name="planning_scene_tutorial", + package="moveit2_tutorials", + executable="planning_scene_tutorial", + output="screen", + parameters=[robot_description, robot_description_semantic, kinematics_yaml], + ) + + return LaunchDescription([planning_scene_tutorial]) diff --git a/doc/planning_scene/planning_scene_tutorial.rst b/doc/planning_scene/planning_scene_tutorial.rst index 8c522e2a72..87e0a8c362 100644 --- a/doc/planning_scene/planning_scene_tutorial.rst +++ b/doc/planning_scene/planning_scene_tutorial.rst @@ -1,8 +1,3 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - Planning Scene ================================== @@ -28,7 +23,7 @@ Running the code ---------------- Roslaunch the launch file to run the code directly from moveit_tutorials: :: - roslaunch moveit_tutorials planning_scene_tutorial.launch + ros2 launch moveit2_tutorials planning_scene_tutorial.launch.py Expected Output --------------- @@ -36,19 +31,19 @@ Expected Output The output should look something like this, though we are using random joint values so some things may be different. :: - ros.moveit_tutorials: Test 1: Current state is not in self collision - ros.moveit_tutorials: Test 2: Current state is not in self collision - ros.moveit_tutorials: Test 3: Current state is not in self collision - ros.moveit_tutorials: Test 4: Current state is valid - ros.moveit_tutorials: Test 5: Current state is in self collision - ros.moveit_tutorials: Contact between: panda_leftfinger and panda_link1 - ros.moveit_tutorials: Contact between: panda_link1 and panda_rightfinger - ros.moveit_tutorials: Test 6: Current state is not in self collision - ros.moveit_tutorials: Test 7: Current state is not in self collision - ros.moveit_tutorials: Test 8: Random state is not constrained - ros.moveit_tutorials: Test 9: Random state is not constrained - ros.moveit_tutorials: Test 10: Random state is not constrained - ros.moveit_tutorials: Test 11: Random state is feasible - ros.moveit_tutorials: Test 12: Random state is not valid - -**Note:** Don't worry if your output has different ROS console format. You can customize your ROS console logger by following `this blog post `_. + moveit2_tutorials: Test 1: Current state is not in self collision + moveit2_tutorials: Test 2: Current state is not in self collision + moveit2_tutorials: Test 3: Current state is not in self collision + moveit2_tutorials: Test 4: Current state is valid + moveit2_tutorials: Test 5: Current state is in self collision + moveit2_tutorials: Contact between: panda_leftfinger and panda_link1 + moveit2_tutorials: Contact between: panda_link1 and panda_rightfinger + moveit2_tutorials: Test 6: Current state is not in self collision + moveit2_tutorials: Test 7: Current state is not in self collision + moveit2_tutorials: Test 8: Random state is not constrained + moveit2_tutorials: Test 9: Random state is not constrained + moveit2_tutorials: Test 10: Random state is not constrained + moveit2_tutorials: Test 11: Random state is feasible + moveit2_tutorials: Test 12: Random state is not valid + +**Note:** Don't worry if your output has different ROS console format. You can customize your ROS console logger by following `this tutorial `_. diff --git a/doc/planning_scene/src/planning_scene_tutorial.cpp b/doc/planning_scene/src/planning_scene_tutorial.cpp index 58b1576703..4f5718b603 100644 --- a/doc/planning_scene/src/planning_scene_tutorial.cpp +++ b/doc/planning_scene/src/planning_scene_tutorial.cpp @@ -1,40 +1,4 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Sachin Chitta, Michael Lautman */ - -#include +#include // MoveIt #include @@ -56,11 +20,18 @@ bool stateFeasibilityTestExample(const moveit::core::RobotState& kinematic_state } // END_SUB_TUTORIAL +static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_tutorial_"); + int main(int argc, char** argv) { - ros::init(argc, argv, "panda_arm_kinematics"); - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto planning_scene_tutorial_node = rclcpp::Node::make_shared("planning_scene_tutorial", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(planning_scene_tutorial_node); + std::thread([&executor]() { executor.spin(); }).detach(); // BEGIN_TUTORIAL // @@ -77,7 +48,7 @@ int main(int argc, char** argv) // this tutorial, we will instantiate a PlanningScene class directly, // but this method of instantiation is only intended for illustration. - robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); + robot_model_loader::RobotModelLoader robot_model_loader(planning_scene_tutorial_node, "robot_description"); const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); planning_scene::PlanningScene planning_scene(kinematic_model); @@ -102,8 +73,7 @@ int main(int argc, char** argv) collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; planning_scene.checkSelfCollision(collision_request, collision_result); - ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); - + RCLCPP_INFO_STREAM(LOGGER, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Change the state // ~~~~~~~~~~~~~~~~ // @@ -118,7 +88,7 @@ int main(int argc, char** argv) current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); - ROS_INFO_STREAM("Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Checking for a group // ~~~~~~~~~~~~~~~~~~~~ @@ -133,7 +103,7 @@ int main(int argc, char** argv) current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); - ROS_INFO_STREAM("Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Getting Contact Information // ~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -146,7 +116,7 @@ int main(int argc, char** argv) std::vector joint_values = { 0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0 }; const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup("panda_arm"); current_state.setJointGroupPositions(joint_model_group, joint_values); - ROS_INFO_STREAM("Test 4: Current state is " + RCLCPP_INFO_STREAM(LOGGER, "Test 4: Current state is " << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid")); // Now, we can get contact information for any collisions that might @@ -162,11 +132,11 @@ int main(int argc, char** argv) collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); - ROS_INFO_STREAM("Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); collision_detection::CollisionResult::ContactMap::const_iterator it; for (it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) { - ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str()); + RCLCPP_INFO(LOGGER, "Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str()); } // Modifying the Allowed Collision Matrix @@ -195,7 +165,7 @@ int main(int argc, char** argv) } collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm); - ROS_INFO_STREAM("Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Full Collision Checking // ~~~~~~~~~~~~~~~~~~~~~~~ @@ -210,7 +180,7 @@ int main(int argc, char** argv) // from obstacles in the environment. collision_result.clear(); planning_scene.checkCollision(collision_request, collision_result, copied_state, acm); - ROS_INFO_STREAM("Test 7: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 7: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Constraint Checking // ^^^^^^^^^^^^^^^^^^^ @@ -237,13 +207,13 @@ int main(int argc, char** argv) std::string end_effector_name = joint_model_group->getLinkModelNames().back(); - geometry_msgs::PoseStamped desired_pose; + geometry_msgs::msg::PoseStamped desired_pose; desired_pose.pose.orientation.w = 1.0; desired_pose.pose.position.x = 0.3; desired_pose.pose.position.y = -0.185; desired_pose.pose.position.z = 0.5; desired_pose.header.frame_id = "panda_link0"; - moveit_msgs::Constraints goal_constraint = + moveit_msgs::msg::Constraints goal_constraint = kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose); // Now, we can check a state against this constraint using the @@ -252,7 +222,7 @@ int main(int argc, char** argv) copied_state.setToRandomPositions(); copied_state.update(); bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint); - ROS_INFO_STREAM("Test 8: Random state is " << (constrained ? "constrained" : "not constrained")); + RCLCPP_INFO_STREAM(LOGGER, "Test 8: Random state is " << (constrained ? "constrained" : "not constrained")); // There's a more efficient way of checking constraints (when you want // to check the same constraint over and over again, e.g. inside a @@ -263,14 +233,14 @@ int main(int argc, char** argv) kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model); kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms()); bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set); - ROS_INFO_STREAM("Test 9: Random state is " << (constrained_2 ? "constrained" : "not constrained")); + RCLCPP_INFO_STREAM(LOGGER, "Test 9: Random state is " << (constrained_2 ? "constrained" : "not constrained")); // There's a direct way to do this using the KinematicConstraintSet // class. kinematic_constraints::ConstraintEvaluationResult constraint_eval_result = kinematic_constraint_set.decide(copied_state); - ROS_INFO_STREAM("Test 10: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained")); + RCLCPP_INFO_STREAM(LOGGER, "Test 10: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained")); // User-defined constraints // ~~~~~~~~~~~~~~~~~~~~~~~~ @@ -282,20 +252,20 @@ int main(int argc, char** argv) planning_scene.setStateFeasibilityPredicate(stateFeasibilityTestExample); bool state_feasible = planning_scene.isStateFeasible(copied_state); - ROS_INFO_STREAM("Test 11: Random state is " << (state_feasible ? "feasible" : "not feasible")); + RCLCPP_INFO_STREAM(LOGGER, "Test 11: Random state is " << (state_feasible ? "feasible" : "not feasible")); // Whenever isStateValid is called, three checks are conducted: (a) // collision checking (b) constraint checking and (c) feasibility // checking using the user-defined callback. bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "panda_arm"); - ROS_INFO_STREAM("Test 12: Random state is " << (state_valid ? "valid" : "not valid")); + RCLCPP_INFO_STREAM(LOGGER, "Test 12: Random state is " << (state_valid ? "valid" : "not valid")); // Note that all the planners available through MoveIt and OMPL will // currently perform collision checking, constraint checking and // feasibility checking using user-defined callbacks. // END_TUTORIAL - ros::shutdown(); + rclcpp::shutdown(); return 0; } From 84e3dc55124fb1d5c37b771e2c5db3c4fac25d62 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 15 Apr 2021 11:05:20 +0000 Subject: [PATCH 2/7] Add pre commit --- .pre-commit-config.yaml | 44 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 .pre-commit-config.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000000..7519e72468 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,44 @@ + +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-merge-conflict + - id: check-symlinks + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + + - repo: https://github.com/psf/black + rev: 20.8b1 + hooks: + - id: black + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-10 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] From 087cb0719aa91e249654d51e33cc52b78c55803b Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 15 Apr 2021 11:10:38 +0000 Subject: [PATCH 3/7] Fix formatting --- .../src/planning_scene_tutorial.cpp | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/doc/planning_scene/src/planning_scene_tutorial.cpp b/doc/planning_scene/src/planning_scene_tutorial.cpp index 4f5718b603..910aa26626 100644 --- a/doc/planning_scene/src/planning_scene_tutorial.cpp +++ b/doc/planning_scene/src/planning_scene_tutorial.cpp @@ -73,7 +73,8 @@ int main(int argc, char** argv) collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; planning_scene.checkSelfCollision(collision_request, collision_result); - RCLCPP_INFO_STREAM(LOGGER, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in") + << " self collision"); // Change the state // ~~~~~~~~~~~~~~~~ // @@ -88,7 +89,8 @@ int main(int argc, char** argv) current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); - RCLCPP_INFO_STREAM(LOGGER, "Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 2: Current state is " << (collision_result.collision ? "in" : "not in") + << " self collision"); // Checking for a group // ~~~~~~~~~~~~~~~~~~~~ @@ -103,7 +105,8 @@ int main(int argc, char** argv) current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); - RCLCPP_INFO_STREAM(LOGGER, "Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 3: Current state is " << (collision_result.collision ? "in" : "not in") + << " self collision"); // Getting Contact Information // ~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -117,7 +120,7 @@ int main(int argc, char** argv) const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup("panda_arm"); current_state.setJointGroupPositions(joint_model_group, joint_values); RCLCPP_INFO_STREAM(LOGGER, "Test 4: Current state is " - << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid")); + << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid")); // Now, we can get contact information for any collisions that might // have happened at a given configuration of the Panda arm. We can ask @@ -132,7 +135,8 @@ int main(int argc, char** argv) collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); - RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in") + << " self collision"); collision_detection::CollisionResult::ContactMap::const_iterator it; for (it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) { @@ -165,7 +169,8 @@ int main(int argc, char** argv) } collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm); - RCLCPP_INFO_STREAM(LOGGER, "Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 6: Current state is " << (collision_result.collision ? "in" : "not in") + << " self collision"); // Full Collision Checking // ~~~~~~~~~~~~~~~~~~~~~~~ @@ -180,7 +185,8 @@ int main(int argc, char** argv) // from obstacles in the environment. collision_result.clear(); planning_scene.checkCollision(collision_request, collision_result, copied_state, acm); - RCLCPP_INFO_STREAM(LOGGER, "Test 7: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); + RCLCPP_INFO_STREAM(LOGGER, "Test 7: Current state is " << (collision_result.collision ? "in" : "not in") + << " self collision"); // Constraint Checking // ^^^^^^^^^^^^^^^^^^^ @@ -240,7 +246,8 @@ int main(int argc, char** argv) kinematic_constraints::ConstraintEvaluationResult constraint_eval_result = kinematic_constraint_set.decide(copied_state); - RCLCPP_INFO_STREAM(LOGGER, "Test 10: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained")); + RCLCPP_INFO_STREAM(LOGGER, "Test 10: Random state is " + << (constraint_eval_result.satisfied ? "constrained" : "not constrained")); // User-defined constraints // ~~~~~~~~~~~~~~~~~~~~~~~~ From 415e5840c949bd6cf8485c9144f129704dd18e52 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 15 Apr 2021 11:37:59 +0000 Subject: [PATCH 4/7] Fix typo --- doc/planning_scene/planning_scene_tutorial.rst | 2 +- doc/planning_scene/src/planning_scene_tutorial.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/planning_scene/planning_scene_tutorial.rst b/doc/planning_scene/planning_scene_tutorial.rst index 87e0a8c362..9638aa5a6a 100644 --- a/doc/planning_scene/planning_scene_tutorial.rst +++ b/doc/planning_scene/planning_scene_tutorial.rst @@ -31,7 +31,7 @@ Expected Output The output should look something like this, though we are using random joint values so some things may be different. :: - moveit2_tutorials: Test 1: Current state is not in self collision + moveit2_tutorials: Test 1: Current state is in self collision moveit2_tutorials: Test 2: Current state is not in self collision moveit2_tutorials: Test 3: Current state is not in self collision moveit2_tutorials: Test 4: Current state is valid diff --git a/doc/planning_scene/src/planning_scene_tutorial.cpp b/doc/planning_scene/src/planning_scene_tutorial.cpp index 910aa26626..112f39fc65 100644 --- a/doc/planning_scene/src/planning_scene_tutorial.cpp +++ b/doc/planning_scene/src/planning_scene_tutorial.cpp @@ -20,7 +20,7 @@ bool stateFeasibilityTestExample(const moveit::core::RobotState& kinematic_state } // END_SUB_TUTORIAL -static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_tutorial_"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_tutorial"); int main(int argc, char** argv) { From e77810ec288b21dec08f0d2ed644d571974e50a4 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 20 Apr 2021 20:56:39 +0000 Subject: [PATCH 5/7] Remove empty line on pre commit --- .pre-commit-config.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7519e72468..b380f58c88 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,4 +1,3 @@ - # To use: # # pre-commit run -a From 3695b33400ac1278b595f613dfe403c9606167cf Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 21 Apr 2021 14:14:01 +0000 Subject: [PATCH 6/7] Fix conflict --- doc/planning_scene/CMakeLists.txt | 2 +- doc/planning_scene/launch/planning_scene_tutorial.launch.py | 1 + doc/planning_scene/planning_scene_tutorial.rst | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/doc/planning_scene/CMakeLists.txt b/doc/planning_scene/CMakeLists.txt index 5d58961926..33cda665fa 100644 --- a/doc/planning_scene/CMakeLists.txt +++ b/doc/planning_scene/CMakeLists.txt @@ -10,4 +10,4 @@ install(TARGETS planning_scene_tutorial ) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} -) \ No newline at end of file +) diff --git a/doc/planning_scene/launch/planning_scene_tutorial.launch.py b/doc/planning_scene/launch/planning_scene_tutorial.launch.py index 1b5dc8b21a..ed2d30eda0 100644 --- a/doc/planning_scene/launch/planning_scene_tutorial.launch.py +++ b/doc/planning_scene/launch/planning_scene_tutorial.launch.py @@ -4,6 +4,7 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory + def load_file(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path) diff --git a/doc/planning_scene/planning_scene_tutorial.rst b/doc/planning_scene/planning_scene_tutorial.rst index 55e7842628..5c6b7bb68c 100644 --- a/doc/planning_scene/planning_scene_tutorial.rst +++ b/doc/planning_scene/planning_scene_tutorial.rst @@ -65,4 +65,4 @@ Optional: add the previous command to your ``.bashrc``: :: Next Step ^^^^^^^^^^ -`Visualize a robot with the interactive motion planning plugin for RViz <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_ \ No newline at end of file +`Visualize a robot with the interactive motion planning plugin for RViz <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_ From 418f37a5422fe4e89392db9fdbf6ea3c70e3b562 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 21 Apr 2021 14:17:42 +0000 Subject: [PATCH 7/7] Fix tutorial rst --- .../planning_scene_tutorial.rst | 113 ++++++++---------- 1 file changed, 47 insertions(+), 66 deletions(-) diff --git a/doc/planning_scene/planning_scene_tutorial.rst b/doc/planning_scene/planning_scene_tutorial.rst index 5c6b7bb68c..9638aa5a6a 100644 --- a/doc/planning_scene/planning_scene_tutorial.rst +++ b/doc/planning_scene/planning_scene_tutorial.rst @@ -1,68 +1,49 @@ -Getting Started -=============== - -This tutorial will install MoveIt2 and create a workspace sandbox to run the tutorials and example robot. - -Install ROS2 and Colcon -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -`Install ROS2 Foxy `_. -It is easy to miss steps when going through the ROS2 installation tutorial. If you run into errors in the next few steps, a good place to start is to go back and make sure you have installed ROS2 correctly. - -Install `rosdep2 `_ to install system dependencies : :: - - sudo apt install python3-rosdep2 - -Once you have ROS2 installed, make sure you have the most up to date packages: :: - - rosdep update - sudo apt update - sudo apt dist-upgrade - -Install `Colcon `_ the ROS2 build system: :: - - sudo apt install python3-colcon-common-extensions - -Install `vcstool `_ : :: - - sudo apt install python3-vcstool - -Create A Colcon Workspace and Download MoveIt2 Source -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -These tutorials rely on the master branch of MoveIt2, which requires a build from source. -You will need to have a `colcon `_ workspace setup: :: +Planning Scene +================================== - mkdir -p ~/ws_moveit2/src - cd ~/ws_moveit2/src +The :planning_scene:`PlanningScene` class provides the main interface that you will use +for collision checking and constraint checking. In this tutorial, we +will explore the C++ interface to this class. - wget https://raw.githubusercontent.com/ros-planning/moveit2/main/moveit2.repos - vcs import < moveit2.repos - wget https://raw.githubusercontent.com/ros-planning/moveit2/main/moveit2.repos - vcs import < moveit2.repos - -Build your Colcon Workspace -^^^^^^^^^^^^^^^^^^^^^^^^^^^ -The following will install from Debian any package dependencies not already in your workspace: :: - - cd ~/ws_moveit2/src - rosdep install -r --from-paths . --ignore-src --rosdistro foxy -y - -The next command will configure your colcon workspace: :: - - cd ~/ws_moveit2 - colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release - -Source the colcon workspace: :: - - source ~/ws_moveit2/install/setup.bash - -Optional: add the previous command to your ``.bashrc``: :: - - echo 'source ~/ws_moveit2/install/setup.bash' >> ~/.bashrc - -.. note:: Sourcing the ``setup.bash`` automatically in your ``~/.bashrc`` is - not required and often skipped by advanced users who use more than one - colcon workspace at a time, but we recommend it for simplicity. - -Next Step -^^^^^^^^^^ -`Visualize a robot with the interactive motion planning plugin for RViz <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_ +Getting Started +--------------- +If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_. + +The entire code +--------------- +The entire code can be seen :codedir:`here in the MoveIt GitHub project`. + +.. tutorial-formatter:: ./src/planning_scene_tutorial.cpp + +The launch file +--------------- +The entire launch file is :codedir:`here ` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package. + +Running the code +---------------- +Roslaunch the launch file to run the code directly from moveit_tutorials: :: + + ros2 launch moveit2_tutorials planning_scene_tutorial.launch.py + +Expected Output +--------------- + +The output should look something like this, though we are using random +joint values so some things may be different. :: + + moveit2_tutorials: Test 1: Current state is in self collision + moveit2_tutorials: Test 2: Current state is not in self collision + moveit2_tutorials: Test 3: Current state is not in self collision + moveit2_tutorials: Test 4: Current state is valid + moveit2_tutorials: Test 5: Current state is in self collision + moveit2_tutorials: Contact between: panda_leftfinger and panda_link1 + moveit2_tutorials: Contact between: panda_link1 and panda_rightfinger + moveit2_tutorials: Test 6: Current state is not in self collision + moveit2_tutorials: Test 7: Current state is not in self collision + moveit2_tutorials: Test 8: Random state is not constrained + moveit2_tutorials: Test 9: Random state is not constrained + moveit2_tutorials: Test 10: Random state is not constrained + moveit2_tutorials: Test 11: Random state is feasible + moveit2_tutorials: Test 12: Random state is not valid + +**Note:** Don't worry if your output has different ROS console format. You can customize your ROS console logger by following `this tutorial `_.