diff --git a/panther_bringup/README.md b/panther_bringup/README.md index 3f0617ff..67c4a64d 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -27,4 +27,5 @@ The package containing default configuration and launch files necessary to start - `user_animations_file` [*string*, default=**None**]: optional parameter with path to yaml file with user defined animations. - `wheel_config_file` [*string*, default=**$(find panther_description)/config/WH01.yaml**]: path to YAML file with wheel specyfication. value of this argument depends on `wheel_type` launch argument. - `wheel_type` [*string*, default=**WH01**]: type of wheel, possible are: **WH01** - offroad, **WH02** - mecanum, **WH04** - small pneumatic, **custom** - custom wheel type (requires setting `wheel_config_file` argument accordingly). -- `use_ekf` [*bool*, default=**true**]: enable or disable Extended Kalman Filter. Keep in mind parameters in [panther_common.yaml](./config/panther_common.yaml) and in [ekf_config.yaml](./config/ekf_config.yaml) are separate and are not affected by this parameter. Especially parameters such as `publish_tf` and TF frames are separate for both nodes and have to be changed independently. \ No newline at end of file +- `exit_on_wrong_hw` [*bool*, default=**true**]: if set to *true* and incorrect hardware is detected kill the entire roslaunch. If set to *false* do not launch nodes and only spin dummy welcome_msg_node. +- `use_ekf` [*bool*, default=**true**]: enable or disable Extended Kalman Filter. Keep in mind parameters in [panther_common.yaml](./config/panther_common.yaml) and in [ekf_config.yaml](./config/ekf_config.yaml) are separate and are not affected by this parameter. Especially parameters such as `publish_tf` and TF frames are separate for both nodes and have to be changed independently. diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index 0febad8c..b1d9c7b6 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -9,6 +9,7 @@ + @@ -19,56 +20,64 @@ - - + + + - + + + + - - - - - + - - - - - + + + + + - - - + + + + + - - - + + + - - - - + + + - + + + + - + + + - + - - - + + + + + diff --git a/panther_bringup/package.xml b/panther_bringup/package.xml index 3a6d5fb7..cf6c2467 100644 --- a/panther_bringup/package.xml +++ b/panther_bringup/package.xml @@ -26,4 +26,7 @@ phidgets_spatial robot_localization + + python3-click + \ No newline at end of file diff --git a/panther_bringup/src/welcome_msg_node.py b/panther_bringup/src/welcome_msg_node.py new file mode 100755 index 00000000..8c3fe9f0 --- /dev/null +++ b/panther_bringup/src/welcome_msg_node.py @@ -0,0 +1,69 @@ +#!/usr/bin/python3 + +import click +import os +import textwrap + +import rospy +import rospkg + + +class WelcomMsgNode: + PANTHER_TEXT = ''' + ____ _ _ + | _ \ __ _ _ __ | |_| |__ ___ _ __ + | |_) / _` | '_ \| __| '_ \ / _ \ '__| + | __/ (_| | | | | |_| | | | __/ | + |_| \__,_|_| |_|\__|_| |_|\___|_| + + ''' + ERROR_MESSAGE = ''' + OS detected incorrect hardware. ROS nodes are prevented from starting! + Refer to instructions in manual or those shown on terminal login. + ''' + + def __init__(self, name: str) -> None: + rospy.init_node(name, anonymous=False) + + correct_hw_env = os.environ.get('PANTHER_HW_CONFIG_CORRECT') + + exit_on_wrong_hw = rospy.get_param('~exit_on_wrong_hw', True) + + rospack = rospkg.RosPack() + stats_to_show = { + 'Serial number': rospy.get_param('/panther/serial_no', '----'), + 'Robot version': rospy.get_param('/panther/robot_version', '1.0'), + 'ROS driver version': rospack.get_manifest('panther').version, + 'Website': 'https://husarion.com', + 'Support': 'https://community.husarion.com/', + 'Bugtracker': 'https://github.com/husarion/panther_ros/issues', + } + + pth_txt = textwrap.dedent(WelcomMsgNode.PANTHER_TEXT) + stats_msg = click.style(pth_txt, bold=True) + ''.join( + [f'{click.style(name, bold=True)}: {value}\n' for name, value in stats_to_show.items()] + ) + rospy.loginfo(f'[{rospy.get_name()}] Panther statistics: {stats_msg}') + + if not correct_hw_env or correct_hw_env.lower() == 'false': + for msg in textwrap.dedent(WelcomMsgNode.ERROR_MESSAGE).strip('\n').split('\n'): + rospy.logerr(f'[{rospy.get_name()}] {msg}') + + if exit_on_wrong_hw: + rospy.signal_shutdown('Panther configuration is incorrect!') + return + + else: + rospy.loginfo(f'[{rospy.get_name()}] Panther configuration is correct') + + +def main(): + welcome_msg_node = WelcomMsgNode('welcome_msg_node') + rospy.spin() + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass