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..33cda665fa 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} +) 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..ed2d30eda0 --- /dev/null +++ b/doc/planning_scene/launch/planning_scene_tutorial.launch.py @@ -0,0 +1,57 @@ +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 7b23789b07..9638aa5a6a 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 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..112f39fc65 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,8 @@ 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 +89,8 @@ 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 +105,8 @@ 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,8 +119,8 @@ 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 " - << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid")); + 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 // have happened at a given configuration of the Panda arm. We can ask @@ -162,11 +135,12 @@ 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 +169,8 @@ 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 +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); - 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 +213,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 +228,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 +239,15 @@ 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 +259,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; }