diff --git a/clearpath_config_live/clearpath_config_live/clearpath_config_live b/clearpath_config_live/clearpath_config_live/clearpath_config_live index c358ba5..9d5e305 100755 --- a/clearpath_config_live/clearpath_config_live/clearpath_config_live +++ b/clearpath_config_live/clearpath_config_live/clearpath_config_live @@ -14,9 +14,11 @@ # 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 @@ -33,12 +35,12 @@ def main(): # Node rclpy.init() node = rclpy.create_node("clearpath_config_live") - node.declare_parameter("config_file", "/etc/clearpath/robot.yaml") + node.declare_parameter("setup_path", "/etc/clearpath") logger = rclpy.logging.get_logger("clearpath_config_live") # Get File - config_param = node.get_parameter("config_file") - config_file = config_param.get_parameter_value().string_value + setup_path_param = node.get_parameter("setup_path") + setup_path = setup_path_param.get_parameter_value().string_value # Watcher, Handler, and Client client = RobotDescriptionClient( @@ -46,7 +48,7 @@ def main(): "robot_state_publisher", "robot_description" ) - watcher = ClearpathConfigWatcher(config_file, logger) + watcher = ClearpathConfigWatcher(setup_path, logger) handler = ClearpathConfigHandler(watcher, client, logger) # Start Handler diff --git a/clearpath_config_live/clearpath_config_live/clearpath_config_updater.py b/clearpath_config_live/clearpath_config_live/clearpath_config_updater.py index 9e64388..e21b2b4 100644 --- a/clearpath_config_live/clearpath_config_live/clearpath_config_updater.py +++ b/clearpath_config_live/clearpath_config_live/clearpath_config_updater.py @@ -23,12 +23,11 @@ class ClearpathConfigUpdater: def __init__( self, - config_file: str, - output_path: str = "/etc/clearpath", + setup_path: str = '/etc/clearpath', ) -> None: - self.config_file = os.path.realpath(config_file) - self.output_path = os.path.realpath(output_path) - self.dirs = {os.path.dirname(self.config_file)} + 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.doc = None def get_robot_description(self): @@ -43,11 +42,11 @@ def update(self) -> None: """Re-load File and Create Description.""" # Generate URDF dg = DescriptionGenerator( - setup_path=self.output_path + setup_path=self.setup_path ) dg.generate() # Re-load Description self.doc = xacro.process_file( - os.path.join(self.output_path, "robot.urdf.xacro") + os.path.join(self.setup_path, "robot.urdf.xacro") ) diff --git a/clearpath_config_live/clearpath_config_live/clearpath_config_watcher.py b/clearpath_config_live/clearpath_config_live/clearpath_config_watcher.py index ad2e37b..18c370e 100644 --- a/clearpath_config_live/clearpath_config_live/clearpath_config_watcher.py +++ b/clearpath_config_live/clearpath_config_live/clearpath_config_watcher.py @@ -22,9 +22,9 @@ class ClearpathConfigWatcher: - def __init__(self, config_file: str, logger) -> None: + def __init__(self, setup_path: str, logger) -> None: self.observer = Observer() - self.updater = ClearpathConfigUpdater(config_file) + self.updater = ClearpathConfigUpdater(setup_path) self.logger = logger @property diff --git a/clearpath_viz/launch/view_diagnostics.launch.py b/clearpath_viz/launch/view_diagnostics.launch.py new file mode 100644 index 0000000..1149d5e --- /dev/null +++ b/clearpath_viz/launch/view_diagnostics.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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 launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node, PushRosNamespace +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Launch Configurations + namespace = LaunchConfiguration('namespace') + + # Launch Arguments + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Robot namespace' + ) + + group_view_model = GroupAction([ + PushRosNamespace(namespace), + Node(package='rqt_robot_monitor', + executable='rqt_robot_monitor', + remappings=[ + ('/diagnostics', 'diagnostics'), + ('/diagnostics_agg', 'diagnostics_agg') + ], + output='screen') + ]) + + ld = LaunchDescription() + # Args + ld.add_action(arg_namespace) + + # Nodes + ld.add_action(group_view_model) + return ld diff --git a/clearpath_viz/launch/view_model.launch.py b/clearpath_viz/launch/view_model.launch.py index ceee9a9..c20f014 100644 --- a/clearpath_viz/launch/view_model.launch.py +++ b/clearpath_viz/launch/view_model.launch.py @@ -1,3 +1,37 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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 launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -13,16 +47,12 @@ def generate_launch_description(): - # Launch Configurations - platform_model = LaunchConfiguration('platform_model') - namespace = LaunchConfiguration('namespace') - # Launch Arguments - arg_platform_model = DeclareLaunchArgument( - 'platform_model', - choices=['a200', 'j100'], - default_value='a200' + arg_setup_path = DeclareLaunchArgument( + 'setup_path', + default_value='/etc/clearpath/' ) + arg_namespace = DeclareLaunchArgument( 'namespace', default_value='', @@ -40,26 +70,32 @@ def generate_launch_description(): default_value='model.rviz', ) + # Launch Configurations + rviz_config = LaunchConfiguration('config') + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + setup_path = LaunchConfiguration('setup_path') + + # Get Package Paths pkg_clearpath_platform_description = FindPackageShare('clearpath_platform_description') pkg_clearpath_viz = FindPackageShare('clearpath_viz') config_rviz = PathJoinSubstitution( - [pkg_clearpath_viz, 'rviz', LaunchConfiguration('config')] + [pkg_clearpath_viz, 'rviz', rviz_config] ) - group_view_model = GroupAction([ PushRosNamespace(namespace), Node(package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', config_rviz], - parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], - remappings=[ + executable='rviz2', + name='rviz2', + arguments=['-d', config_rviz], + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static') - ], - output='screen'), + ], + output='screen'), Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', @@ -67,24 +103,28 @@ def generate_launch_description(): ("joint_states", "platform/joint_states") ] ), - + # Load Robot Description IncludeLaunchDescription( PathJoinSubstitution([ pkg_clearpath_platform_description, 'launch', 'description.launch.py']) ), + # Live Updater Node( package='clearpath_config_live', executable='clearpath_config_live', + parameters=[{'setup_path': setup_path}] ), ]) + # Generate Initial Description node_generate_description = Node( package='clearpath_generator_common', executable='generate_description', name='generate_description', output='screen', + arguments=['-s', setup_path], ) event_generate_description = RegisterEventHandler( @@ -96,7 +136,7 @@ def generate_launch_description(): ld = LaunchDescription() # Args - ld.add_action(arg_platform_model) + ld.add_action(arg_setup_path) ld.add_action(arg_namespace) ld.add_action(arg_rviz_config) ld.add_action(arg_use_sim_time) diff --git a/clearpath_viz/launch/view_navigation.launch.py b/clearpath_viz/launch/view_navigation.launch.py index c08142f..13d6a3e 100644 --- a/clearpath_viz/launch/view_navigation.launch.py +++ b/clearpath_viz/launch/view_navigation.launch.py @@ -1,3 +1,37 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution diff --git a/clearpath_viz/launch/view_robot.launch.py b/clearpath_viz/launch/view_robot.launch.py index 795b862..f3e8629 100644 --- a/clearpath_viz/launch/view_robot.launch.py +++ b/clearpath_viz/launch/view_robot.launch.py @@ -1,3 +1,37 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -37,15 +71,15 @@ def generate_launch_description(): group_view_model = GroupAction([ PushRosNamespace(namespace), Node(package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', config_rviz], - parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], - remappings=[ - ('/tf', 'tf'), - ('/tf_static', 'tf_static') - ], - output='screen') + executable='rviz2', + name='rviz2', + arguments=['-d', config_rviz], + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ], + output='screen') ]) diff --git a/clearpath_viz/package.xml b/clearpath_viz/package.xml index 757cda2..ec6a3a3 100644 --- a/clearpath_viz/package.xml +++ b/clearpath_viz/package.xml @@ -14,6 +14,7 @@ clearpath_platform_description joint_state_publisher_gui rviz2 + rqt_robot_monitor ament_cmake