From db9510c07942da6c9c015729fed5d55d2f2df1b7 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Fri, 21 Jul 2023 14:10:25 +0200 Subject: [PATCH 1/5] Add node preventing start of incorrect hardware --- panther_bringup/launch/bringup.launch | 83 ++++++++++++++----------- panther_bringup/package.xml | 3 + panther_bringup/src/welcome_msg_node.py | 69 ++++++++++++++++++++ 3 files changed, 118 insertions(+), 37 deletions(-) create mode 100755 panther_bringup/src/welcome_msg_node.py diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index d969af727..609c72b2b 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -9,6 +9,7 @@ + - - + + + - + + + + - - - - - + - - - - - + + + + + - - - + + + + + - - - + + + - - - - + + + - + + + + - + - + + + - - - + + + + + diff --git a/panther_bringup/package.xml b/panther_bringup/package.xml index 96bd76232..4264b2733 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 000000000..4f39c4810 --- /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 infstructions 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 From 04292a3729622c291195e1b36a0d5716a5d7854d Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 24 Jul 2023 13:50:26 +0200 Subject: [PATCH 2/5] Update panther_bringup/src/welcome_msg_node.py MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_bringup/src/welcome_msg_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_bringup/src/welcome_msg_node.py b/panther_bringup/src/welcome_msg_node.py index 4f39c4810..8c3fe9f06 100755 --- a/panther_bringup/src/welcome_msg_node.py +++ b/panther_bringup/src/welcome_msg_node.py @@ -19,7 +19,7 @@ class WelcomMsgNode: ''' ERROR_MESSAGE = ''' OS detected incorrect hardware. ROS nodes are prevented from starting! - Refer to infstructions in manual or those shown on terminal login. + Refer to instructions in manual or those shown on terminal login. ''' def __init__(self, name: str) -> None: From 3afc8c1a6599bfdee19f39c9752284cd9bd8c447 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Mon, 24 Jul 2023 11:56:17 +0000 Subject: [PATCH 3/5] Reneme parameter and update README --- panther_bringup/README.md | 2 +- panther_bringup/launch/bringup.launch | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_bringup/README.md b/panther_bringup/README.md index 4149c3d1b..87369e820 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -27,4 +27,4 @@ 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). - +- `exit_on_wrong_hw` [*bool*, default=**true**]: if set to *ture* and incorrect hadrware is detected kill entire roslaunch. If set to *false* do not launch nodes and only spin dummy welcome_msg_node. diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index 609c72b2b..291fa9873 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -9,7 +9,7 @@ - + - + From 43ff461746f335e89283058a46f4642100e216a7 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 24 Jul 2023 14:12:36 +0200 Subject: [PATCH 4/5] Update panther_bringup/README.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_bringup/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_bringup/README.md b/panther_bringup/README.md index 87369e820..f20663608 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -27,4 +27,4 @@ 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). -- `exit_on_wrong_hw` [*bool*, default=**true**]: if set to *ture* and incorrect hadrware is detected kill entire roslaunch. If set to *false* do not launch nodes and only spin dummy welcome_msg_node. +- `exit_on_wrong_hw` [*bool*, default=**true**]: if set to *ture* and incorrect hardware is detected kill entire roslaunch. If set to *false* do not launch nodes and only spin dummy welcome_msg_node. From 7539a0e0770cbcf86c3f19c8b015adf90e7a6eb9 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Mon, 24 Jul 2023 14:26:00 +0200 Subject: [PATCH 5/5] Update panther_bringup/README.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_bringup/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_bringup/README.md b/panther_bringup/README.md index 4101a1c81..67c4a64d0 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -27,5 +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). -- `exit_on_wrong_hw` [*bool*, default=**true**]: if set to *ture* and incorrect hardware is detected kill entire roslaunch. If set to *false* do not launch nodes and only spin dummy welcome_msg_node. +- `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.