Merge pull request #15 from clearpathrobotics/0.3.0-RC

0.3 RC

clearpath_config_live/clearpath_config_live/clearpath_config_live

@@ -14,11 +14,9 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import getopt
 import rclpy
 import rclpy.logging
 import rclpy.node
-import sys
 
 from clearpath_config_live.clearpath_config_handler import (
     ClearpathConfigHandler

clearpath_config_live/clearpath_config_live/clearpath_config_updater.py

@@ -23,11 +23,13 @@ class ClearpathConfigUpdater:
 
     def __init__(
             self,
-            setup_path: str = '/etc/clearpath',
+            setup_path: str,
+            logger,
             ) -> None:
+        self.logger = logger
         self.setup_path = os.path.realpath(setup_path)
         self.config_file = os.path.join(self.setup_path, 'robot.yaml')
-        self.dirs = {self.setup_path}
+        self.paths = {self.config_file}
         self.doc = None
 
     def get_robot_description(self):
@@ -45,7 +47,6 @@ def update(self) -> None:
             setup_path=self.setup_path
         )
         dg.generate()
-
         # Re-load Description
         self.doc = xacro.process_file(
             os.path.join(self.setup_path, "robot.urdf.xacro")

clearpath_config_live/clearpath_config_live/clearpath_config_watcher.py

@@ -24,7 +24,7 @@ class ClearpathConfigWatcher:
 
     def __init__(self, setup_path: str, logger) -> None:
         self.observer = Observer()
-        self.updater = ClearpathConfigUpdater(setup_path)
+        self.updater = ClearpathConfigUpdater(setup_path, logger)
         self.logger = logger
 
     @property
@@ -44,13 +44,10 @@ def stop(self) -> None:
 
     def update_watched(self, event_handler: FileSystemEventHandler) -> None:
         """Update Directories being Watched."""
-        if self.watched != self.updater.dirs:
-            self.observer.unschedule_all()
-            for dir in self.updater.dirs:
-                self.logger.info("Watching directory: %s" % dir)
-                self.observer.schedule(event_handler,
-                                       path=dir,
-                                       recursive=False)
+        self.observer.unschedule_all()
+        for path in self.updater.paths:
+            self.logger.info("Watching directory: %s" % dir)
+            self.observer.schedule(event_handler, path=path, recursive=False)
 
     def update(self, event_handler: FileSystemEventHandler) -> None:
         """Update Clearpath Config and Watchlist."""

clearpath_viz/launch/view_model.launch.py

@@ -90,7 +90,7 @@ def generate_launch_description():
              executable='rviz2',
              name='rviz2',
              arguments=['-d', config_rviz],
-             parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
+             parameters=[{'use_sim_time': use_sim_time}],
              remappings=[
                ('/tf', 'tf'),
                ('/tf_static', 'tf_static')

clearpath_viz/launch/view_moveit.launch.py

@@ -0,0 +1,158 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Roni Kreinin <[email protected]>
+# @copyright (c) 2023, Clearpath Robotics, 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 Clearpath Robotics 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 HOLDER 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.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+from pprint import pprint
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node, PushRosNamespace
+from launch_ros.substitutions import FindPackageShare
+
+from clearpath_config.common.utils.yaml import read_yaml, write_yaml
+
+MARKER = 'rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/'
+
+MOVEIT_TOPICS = [
+    'attached_collision_object',
+    'compute_cartesian_path',
+    'display_planned_path',
+    'get_planner_params',
+    'monitored_planning_scene',
+    'move_action',
+    'query_planner_interface',
+    'set_planner_params',
+    'trajectory_execution_event',
+    MARKER + 'feedback',
+    MARKER + 'get_interactive_markers',
+    MARKER + 'update',
+]
+
+
+def launch_setup(context, *args, **kwargs):
+    # Launch Configurations
+    namespace = LaunchConfiguration('namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    arm_count = LaunchConfiguration('arm_count')
+
+    # Apply Namespace to Rviz Configuration
+    context_namespace = namespace.perform(context)
+
+    # RViz Configuration
+    pkg_clearpath_viz = FindPackageShare('clearpath_viz')
+    default_config = PathJoinSubstitution(
+        [pkg_clearpath_viz, 'rviz', 'moveit.rviz']
+    )
+
+    context_rviz = default_config.perform(context)
+    content_rviz = read_yaml(context_rviz)
+
+    content_rviz[
+        'Visualization Manager'][
+            'Displays'][1][
+                'Move Group Namespace'] = '/' + context_namespace
+
+    namespaced_config = '/tmp/moveit.rviz'
+    write_yaml(namespaced_config, content_rviz)
+
+    # Remappings
+    remappings = [
+        # Standard
+        ('/tf', 'tf'),
+        ('/tf_static', 'tf_static'),
+    ]
+
+    # Remappings for MoveIt!
+    for topic in MOVEIT_TOPICS:
+        # Standard Topics
+        remappings.append(('/%s' % topic, topic))
+        # Doubled Topics
+        remappings.append(('/%s/%s/' % (context_namespace, context_namespace) + topic, topic))
+
+    # Arm Kinematics
+    parameters = {}
+    parameters['use_sim_time'] = use_sim_time
+    parameters['robot_description_kinematics'] = {}
+    for i in range(int(arm_count.perform(context))):
+        parameters['robot_description_kinematics'].update(
+            {'arm_%s' % i: {
+                'kinematics_solver': 'kdl_kinematics_plugin/KDLKinematicsPlugin',
+                'kinematics_solver_search_resolution': 0.005,
+                'kinematics_solver_timeout': 0.005,
+            }}
+        )
+
+    return [
+        GroupAction([
+            PushRosNamespace(namespace),
+            Node(
+                package='rviz2',
+                executable='rviz2',
+                name='rviz2',
+                arguments=['-d', namespaced_config],
+                parameters=[parameters],
+                remappings=remappings,
+                output='screen'
+            )
+        ])
+    ]
+
+
+def generate_launch_description():
+    # Launch Arguments
+    arg_namespace = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Robot namespace'
+    )
+
+    arg_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true'
+    )
+
+    arg_arm_count = DeclareLaunchArgument(
+        'arm_count',
+        default_value='1',
+        description='Number of arms to add kinematics for.'
+    )
+
+    ld = LaunchDescription()
+    # Args
+    ld.add_action(arg_namespace)
+    ld.add_action(arg_use_sim_time)
+    ld.add_action(arg_arm_count)
+    # Nodes
+    ld.add_action(OpaqueFunction(function=launch_setup))
+    return ld

clearpath_viz/launch/view_robot.launch.py

@@ -82,7 +82,6 @@ def generate_launch_description():
              output='screen')
     ])
 
-
     ld = LaunchDescription()
     # Args
     ld.add_action(arg_namespace)

clearpath_viz/rviz/moveit.rviz

@@ -0,0 +1,109 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 70
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MotionPlanning1
+      Splitter Ratio: 0.5
+    Tree Height: 393
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/MotionPlanning
+      Move Group Namespace: ''
+      Name: MotionPlanning
+      Planned Path:
+        Loop Animation: true
+        State Display Time: 0.05 s
+        Trajectory Topic: display_planned_path
+      Planning Scene Topic: monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+      Scene Robot:
+        Robot Alpha: 0.5
+      Value: true
+      Acceleration_Scaling_Factor: 0.5
+      Velocity_Scaling_Factor: 0.5
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 4
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5049999952316284
+      Target Frame: base_link
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 0
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1136
+  Hide Left Dock: false
+  Hide Right Dock: true
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000200000000000002b400000416fc0200000004fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d0000020c000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000024f000002040000017d00fffffffb0000001200530065006c0065006300740069006f006e000000034d0000005c0000005c00ffffff00000001000001cb00000416fc0200000001fb0000000a00560069006500770073000000003d00000416000000a400ffffff0000048e0000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1864
+  X: 56
+  Y: 27