Merge branch 'main' into ch3/trajectory-cache-refactor

.codespell_words

@@ -0,0 +1 @@
+SINIC

.dockerignore

@@ -1,6 +1,5 @@
 __pycache__
 **/.git
-**/.github
 **/_scripts
 **/_static
 **/_templates

.github/upstream.repos

@@ -36,4 +36,4 @@ repositories:
   gz_ros2_control:
     type: git
     url: https://github.com/ros-controls/gz_ros2_control.git
-    version: master
+    version: rolling

.pre-commit-config.yaml

@@ -15,7 +15,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v3.4.0
+    rev: v4.6.0
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict
@@ -28,7 +28,7 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/psf/black
-    rev: 22.3.0
+    rev: 24.8.0
     hooks:
       - id: black
 
@@ -49,7 +49,7 @@ repos:
         args: ['-fallback-style=none', '-i']
 
   - repo: https://github.com/codespell-project/codespell
-    rev: v2.0.0
+    rev: v2.3.0
     hooks:
       - id: codespell
-        args: ['--write-changes']
+        args: ['--write-changes', '--ignore-words=.codespell_words']

CMakeLists.txt

@@ -46,14 +46,14 @@ add_subdirectory(doc/examples/collision_environments)
 # add_subdirectory(doc/examples/interactivity)
 # add_subdirectory(doc/examples/kinematics)
 # add_subdirectory(doc/examples/move_group_python_interface)
-# add_subdirectory(doc/examples/perception_pipeline)
 # add_subdirectory(doc/examples/pick_place)
 # add_subdirectory(doc/examples/planning)
 # add_subdirectory(doc/examples/state_display)
 # add_subdirectory(doc/examples/subframes)
 # add_subdirectory(doc/examples/tests)
 # add_subdirectory(doc/examples/trajopt_planner)
 # add_subdirectory(doc/examples/visualizing_collisions)
+add_subdirectory(doc/examples/perception_pipeline)
 add_subdirectory(doc/examples/jupyter_notebook_prototyping)
 add_subdirectory(doc/examples/motion_planning_api)
 add_subdirectory(doc/examples/motion_planning_pipeline)

doc/examples/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp

@@ -72,7 +72,7 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_
   std::vector<double> start_joint_values;
   start_state->copyJointGroupPositions(joint_model_group, start_joint_values);
 
-  // This planner only supports one goal constriant in the request
+  // This planner only supports one goal constraint in the request
   std::vector<moveit_msgs::Constraints> goal_constraints = req.goal_constraints;
   std::vector<moveit_msgs::JointConstraint> goal_joint_constraint = goal_constraints[0].joint_constraints;
 

doc/examples/hybrid_planning/hybrid_planning_tutorial.rst

@@ -90,7 +90,7 @@ To include the Hybrid Planning Architecture into you project you need to add a *
 
 Customizing the Hybrid Planning Architecture
 --------------------------------------------
-As the rest of MoveIt 2, the *Hybrid Planning Architecture* is designed to be highly customizable while also offering the possibility to easily re-use existing solutions. Each of the architecture's components is a ROS 2 node and can be completely replaced by your own custom ROS 2 node as long as it offers the API required by the other nodes. Each component's runtime behavior is defined by plugins. This section focuses on the customization of the *Hybrid Planning Architecture* by implementing your own plugins.
+As the rest of MoveIt 2, the *Hybrid Planning Architecture* is designed to be highly customizable while also offering the possibility to easily reuse existing solutions. Each of the architecture's components is a ROS 2 node and can be completely replaced by your own custom ROS 2 node as long as it offers the API required by the other nodes. Each component's runtime behavior is defined by plugins. This section focuses on the customization of the *Hybrid Planning Architecture* by implementing your own plugins.
 
 Global and Local Motion Planning
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

doc/examples/jupyter_notebook_prototyping/launch/jupyter_notebook_prototyping.launch.py

@@ -1,6 +1,7 @@
 """
 A launch file that starts a Jupyter notebook server and nodes that support motion planning with the MoveIt Python library.
 """
+
 import os
 import yaml
 from launch import LaunchDescription
@@ -18,7 +19,8 @@ def load_yaml(package_name, 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
+    # parent of IOError, OSError *and* WindowsError where available
+    except EnvironmentError:
         return None
 
 

doc/examples/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py

@@ -13,7 +13,8 @@ def load_file(package_name, file_path):
     try:
         with open(absolute_file_path, "r") as file:
             return file.read()
-    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+    # parent of IOError, OSError *and* WindowsError where available
+    except EnvironmentError:
         return None
 
 
@@ -24,7 +25,8 @@ def load_yaml(package_name, 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
+    # parent of IOError, OSError *and* WindowsError where available
+    except EnvironmentError:
         return None
 
 

doc/examples/motion_planning_python_api/launch/motion_planning_python_api_tutorial.launch.py

@@ -1,6 +1,7 @@
 """
 A launch file for running the motion planning python api tutorial
 """
+
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription

doc/examples/moveit_grasps/moveit_grasps_tutorial.rst

@@ -219,7 +219,7 @@ The ``setIdealGraspPoseRPY()`` and ``setIdealGraspPose()`` methods in GraspGener
 
 These methods is used to score grasp candidates favoring grasps that are closer to the desired orientation.
 
-This is useful in applications such as bin and shelf picking where you would want to pick the objects from a bin with a grasp that is vertically aligned and you would want to pick obejects from a shelf with a grasp that is horozontally aligned.
+This is useful in applications such as bin and shelf picking where you would want to pick the objects from a bin with a grasp that is vertically aligned and you would want to pick objects from a shelf with a grasp that is horizontally aligned.
 
 Tested Robots
 -------------

doc/examples/perception_pipeline/CMakeLists.txt

@@ -1,13 +1,2 @@
-add_executable(cylinder_segment src/cylinder_segment.cpp)
-target_link_libraries(cylinder_segment ${catkin_LIBRARIES})
-
-add_executable(bag_publisher_maintain_time src/bag_publisher_maintain_time.cpp)
-target_link_libraries(bag_publisher_maintain_time ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-install(
-TARGETS
-    bag_publisher_maintain_time
-    cylinder_segment
-DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY urdf launch meshes config worlds rviz2
+  DESTINATION share/moveit2_tutorials)

doc/examples/perception_pipeline/config/sensors_3d.yaml

@@ -0,0 +1,22 @@
+sensors:
+  - camera_1_pointcloud
+  - camera_2_depth_image
+camera_1_pointcloud:
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /camera_1/points
+    max_range: 5.0
+    point_subsample: 1
+    padding_offset: 0.1
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: /camera_1/filtered_points
+camera_2_depth_image:
+    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+    image_topic: /camera_2/depth/image_raw
+    queue_size: 5
+    near_clipping_plane_distance: 0.3
+    far_clipping_plane_distance: 5.0
+    shadow_threshold: 0.2
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: /camera_2/filtered_points

doc/examples/perception_pipeline/launch/depth_camera_environment.launch.py

@@ -0,0 +1,153 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import Command
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import PathJoinSubstitution
+import math
+
+
+def generate_launch_description():
+    urdf_file = os.path.join(
+        get_package_share_directory("moveit2_tutorials"),
+        "urdf/realsense_d435/camera.urdf.xacro",
+    )
+
+    # The list presents the pose in euler coordinates ordered x, y, z, yaw, pitch, roll respectively.
+    camera_1_pose = ["-1", "-1", "0", f"{math.pi/2}", "0.0", "0.0"]
+    camera_2_pose = ["-1", "1", "0", f"-{math.pi/2}", "0.0", "0.0"]
+
+    # It is necessary for gazebo spawner to use the description of camera_1 in gazebo environment
+    camera_1_robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[
+            {
+                "use_sim_time": True,
+                "robot_description": Command(
+                    [f"xacro {urdf_file}", " camera_name:='camera_1'"]
+                ),
+            }
+        ],
+        remappings=[
+            ("robot_description", "/camera_1/robot_description"),
+        ],
+    )
+
+    # It is necessary for spawning camera_1 in gazebo environment
+    camera_1_gazebo_spawner_node = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=[
+            "-entity",
+            "mr_camera",
+            "-topic",
+            "/camera_1/robot_description",
+            "-x",
+            camera_1_pose[0],
+            "-y",
+            camera_1_pose[1],
+            "-z",
+            camera_1_pose[2],
+            "-Y",
+            camera_1_pose[3],
+            "-P",
+            camera_1_pose[4],
+            "-R",
+            camera_1_pose[5],
+        ],
+        output="screen",
+    )
+
+    # It is necessary to make transformation between world frame and camera frames enable later.
+    camera_1_tf_from_world_publisher_node = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=[*camera_1_pose, "world", "camera_1_base_link"],
+    )
+
+    # It is necessary for gazebo spawner to use the description of camera_2 in gazebo environment
+    camera_2_gazebo_spawner_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher2",
+        output="screen",
+        parameters=[
+            {
+                "use_sim_time": True,
+                "robot_description": Command(
+                    [f"xacro {urdf_file}", " camera_name:='camera_2'"]
+                ),
+            }
+        ],
+        remappings=[
+            ("robot_description", "/camera_2/robot_description"),
+        ],
+    )
+
+    # It is necessary for spawning camera_2 in gazebo environment
+    camera_2_robot_state_publisher_node = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        name="spawn2",
+        arguments=[
+            "-entity",
+            "mr_camera2",
+            "-topic",
+            "/camera_2/robot_description",
+            "-x",
+            camera_2_pose[0],
+            "-y",
+            camera_2_pose[1],
+            "-z",
+            camera_2_pose[2],
+            "-Y",
+            camera_2_pose[3],
+            "-P",
+            camera_2_pose[4],
+            "-R",
+            camera_2_pose[5],
+        ],
+        output="screen",
+    )
+
+    # It is necessary to make transformation between world frame and camera frames enable later.
+    camera_2_tf_from_world_publisher_node = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=[*camera_2_pose, "world", "camera_2_base_link"],
+    )
+
+    # It is necessary to open previously created gazebo world for perception pipeline demo.
+    gazebo_launch = IncludeLaunchDescription(
+        PathJoinSubstitution(
+            [FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"]
+        ),
+        launch_arguments={
+            "world": os.path.join(
+                get_package_share_directory("moveit2_tutorials"),
+                "worlds",
+                "perception_pipeline_demo.world",
+            ),
+        }.items(),
+    )
+
+    return LaunchDescription(
+        [
+            camera_1_robot_state_publisher_node,
+            camera_1_gazebo_spawner_node,
+            camera_1_tf_from_world_publisher_node,
+            camera_2_robot_state_publisher_node,
+            camera_2_gazebo_spawner_node,
+            camera_2_tf_from_world_publisher_node,
+            gazebo_launch,
+        ]
+    )