Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add node preventing start of incorrect hardware #147

Merged
merged 6 commits into from
Jul 24, 2023
Merged

Add node preventing start of incorrect hardware #147

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
db9510c
Add node preventing start of incorrect hardware
Kotochleb Jul 21, 2023
04292a3
Update panther_bringup/src/welcome_msg_node.py
Kotochleb Jul 24, 2023
3afc8c1
Reneme parameter and update README
Kotochleb Jul 24, 2023
43ff461
Update panther_bringup/README.md
Kotochleb Jul 24, 2023
c89b9de
Merge branch 'ros1' of github.com:husarion/panther_ros into ros1-inco…
Kotochleb Jul 24, 2023
7539a0e
Update panther_bringup/README.md
Kotochleb Jul 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion panther_bringup/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
- `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.
83 changes: 46 additions & 37 deletions panther_bringup/launch/bringup.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
<arg name="launch_lights_scheduler" default="true" />
<arg name="shutdown_hosts_config_file" default="$(find panther_bringup)/config/shutdown_hosts.yaml" />
<arg name="disable_manager" default="false" doc="used for testing purposes" />
<arg name="exit_on_wrong_hw" default="true" doc="kill ROS when incorrect hardware detected" />
<arg name="use_ekf" default="true" doc="enable or disable EKF" />

<arg name="wheel_config_file" if="$(eval wheel_type == 'custom')" />
Expand All @@ -19,56 +20,64 @@
<arg name="wheel_config_file" value="$(find panther_description)/config/WH04.yaml"
if="$(eval wheel_type == 'WH04')" />

<arg name="robot_description" if="$(arg publish_robot_state)"
default="$(find xacro)/xacro $(find panther_description)/urdf/panther.urdf.xacro
wheel_config_path:=$(find panther_description)/config/$(arg wheel_type).yaml
imu_pos_x:=$(optenv PANTHER_IMU_LOCALIZATION_X 0.168)
imu_pos_y:=$(optenv PANTHER_IMU_LOCALIZATION_Y 0.028)
imu_pos_z:=$(optenv PANTHER_IMU_LOCALIZATION_Z 0.083)
imu_rot_r:=$(optenv PANTHER_IMU_ORIENTATION_R 3.14)
imu_rot_p:=$(optenv PANTHER_IMU_ORIENTATION_P -1.57)
imu_rot_y:=$(optenv PANTHER_IMU_ORIENTATION_Y 0.0)" />

<!-- load default params -->
<rosparam command="load" file="$(find panther_bringup)/config/panther_default.yaml" />
<!-- override default params -->
<rosparam command="load" file="/run/husarion/panther_config.yaml" if="$(optenv PANTHER_HAS_CONFIG false)" />
<node pkg="panther_bringup" type="welcome_msg_node.py" name="welcome_msg_node" output="screen" required="true">
<param name="exit_on_wrong_hw" value="$(arg exit_on_wrong_hw)" />
</node>

<group ns="$(arg namespace)">
<!-- Do not start ROS packages if hardware is detected to be incorrect -->
<group if="$(optenv PANTHER_HW_CONFIG_CORRECT false)">

<arg name="robot_description" if="$(arg publish_robot_state)"
default="$(find xacro)/xacro $(find panther_description)/urdf/panther.urdf.xacro
wheel_config_path:=$(find panther_description)/config/$(arg wheel_type).yaml
imu_pos_x:=$(optenv PANTHER_IMU_LOCALIZATION_X 0.168)
imu_pos_y:=$(optenv PANTHER_IMU_LOCALIZATION_Y 0.028)
imu_pos_z:=$(optenv PANTHER_IMU_LOCALIZATION_Z 0.083)
imu_rot_r:=$(optenv PANTHER_IMU_ORIENTATION_R 3.14)
imu_rot_p:=$(optenv PANTHER_IMU_ORIENTATION_P -1.57)
imu_rot_y:=$(optenv PANTHER_IMU_ORIENTATION_Y 0.0)" />

<include file="$(find panther_driver)/launch/driver.launch">
<arg name="panther_version" value="$(arg panther_version)" />
<arg name="wheel_config_file" value="$(arg wheel_config_file)" />
<arg name="panther_common_config_file" value="$(arg panther_common_config_file)" />
</include>
<group ns="$(arg namespace)">

<include file="$(find panther_lights)/launch/lights.launch">
<arg name="launch_scheduler" value="$(arg launch_lights_scheduler)" />
<arg name="test" value="$(arg test_animations)" />
<arg name="user_animations_file" value="$(arg user_animations_file)" />
</include>
<include file="$(find panther_driver)/launch/driver.launch">
<arg name="panther_version" value="$(arg panther_version)" />
<arg name="wheel_config_file" value="$(arg wheel_config_file)" />
<arg name="panther_common_config_file" value="$(arg panther_common_config_file)" />
</include>

<include file="$(find panther_battery)/launch/battery.launch">
<arg name="panther_version" value="$(arg panther_version)" />
</include>
<include file="$(find panther_lights)/launch/lights.launch">
<arg name="launch_scheduler" value="$(arg launch_lights_scheduler)" />
<arg name="test" value="$(arg test_animations)" />
<arg name="user_animations_file" value="$(arg user_animations_file)" />
</include>

<include file="$(find panther_power_control)/launch/power_control.launch">
<arg name="panther_version" value="$(arg panther_version)" />
</include>
<include file="$(find panther_battery)/launch/battery.launch">
<arg name="panther_version" value="$(arg panther_version)" />
</include>

<include file="$(find panther_manager)/launch/manager.launch" unless="$(arg disable_manager)">
<arg name="panther_version" value="$(arg panther_version)" />
<arg name="shutdown_hosts_config_file" value="$(arg shutdown_hosts_config_file)" />
</include>
<include file="$(find panther_power_control)/launch/power_control.launch">
<arg name="panther_version" value="$(arg panther_version)" />
</include>

<include file="$(find panther_bringup)/launch/imu.launch" />
<include file="$(find panther_manager)/launch/manager.launch" unless="$(arg disable_manager)">
<arg name="panther_version" value="$(arg panther_version)" />
<arg name="shutdown_hosts_config_file" value="$(arg shutdown_hosts_config_file)" />
</include>

<include file="$(find panther_bringup)/launch/ekf.launch" if="$(arg use_ekf)" />
<include file="$(find panther_bringup)/launch/imu.launch" />

<include file="$(find panther_bringup)/launch/ekf.launch" if="$(arg use_ekf)" />

</group>
</group>

<include file="$(find panther_description)/launch/panther_state_publisher.launch" if="$(arg publish_robot_state)">
<arg name="robot_description" value="$(arg robot_description)" />
</include>
<include file="$(find panther_description)/launch/panther_state_publisher.launch" if="$(arg publish_robot_state)">
<arg name="robot_description" value="$(arg robot_description)" />
</include>

</group>

</launch>
3 changes: 3 additions & 0 deletions panther_bringup/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,4 +26,7 @@
<depend>phidgets_spatial</depend>
<depend>robot_localization</depend>

<!-- Python dependencies -->
<depend>python3-click</depend>

</package>
69 changes: 69 additions & 0 deletions panther_bringup/src/welcome_msg_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Loading