Port planning_scene_tutorial

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)

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}
+)

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])

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,27 +23,27 @@ 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
 ---------------
 
 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 <http://dav.ee/blog/notes/archives/898>`_.
+ 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 <https://docs.ros.org/en/foxy/Tutorials/Logging-and-logger-configuration.html#console-output-formatting>`_.

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 <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 // MoveIt
 #include <moveit/robot_model_loader/robot_model_loader.h>
@@ -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<double> 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;
 }