Skip to content

Instantly share code, notes, and snippets.

@guru-florida
Last active March 1, 2020 18:27
Show Gist options
  • Save guru-florida/024f72385aa13cd2beb104ebcfdbc926 to your computer and use it in GitHub Desktop.
Save guru-florida/024f72385aa13cd2beb104ebcfdbc926 to your computer and use it in GitHub Desktop.
ROS2 Humanoid Control and SystemD
#!/usr/bin/python3
#
# Humanoid Service Control
#
# This python script controls an RGB LED connected to the RPi GPIO in order to display ROS2 robot status. It also
# scans a button which can toggle stand-alone or development mode, or on long-hold shutdown the RPi. This may not
# fit your needs, and you could instead just have it call humanoidctl command above on start, stop, status.
#
from gpiozero import LED, Button
from signal import pause
from time import sleep
import subprocess
import pwd
import sys
import os
import signal
Red = LED(21)
Blue = LED(19)
Green = LED(16)
Mode = Button(20, pull_up=True, active_state=None, hold_time=8)
STANDALONE = 0
DEVEL = 1
SHUTDOWN = 2
mode = DEVEL
user_name = 'guru'
ros_proc = None
def shutdown():
global mode, Mode, Red, Green, Blue
mode = SHUTDOWN
print("shutting down")
Blue.off()
Green.off()
Red.on()
if ros_proc is not None:
os.kill(ros_proc.pid, signal.SIGINT)
os.system("poweroff")
def launch_ros():
global ros_proc
print("starting ROS2 production")
if ros_proc is None:
ros_proc = subprocess.Popen(['/usr/bin/humanoidctl', 'start'], close_fds=True)
else:
print("ROS2 already running")
def kill_ros():
global ros_proc
if ros_proc is not None:
print("signaling ROS2 to stop")
os.kill(ros_proc.pid, signal.SIGINT)
ros_proc = None
def standalone():
global Red, Green, Blue
Green.off()
Red.off()
Blue.on()
launch_ros()
def devel():
global Red, Green, Blue
Red.off()
Blue.off()
Green.on()
print("entering development mode")
kill_ros()
def activateMode():
if mode == STANDALONE:
standalone()
elif mode == DEVEL:
devel()
def toggleMode():
global mode, Mode
if mode < SHUTDOWN:
mode = (mode + 1) % 2
activateMode()
def rel():
print("released")
toggleMode()
def pres():
print("pressed")
def signal_sigint(signum, frame):
print("stopping ros2 service")
Red.off()
Blue.off()
Green.off()
sys.exit(0)
Mode.when_pressed = pres
Mode.when_released = rel
Mode.when_held = shutdown
activateMode()
pause()
[Unit]
Description=LSS Humanoid Startup Service
After=NetworkManager.service time-sync.target
[Service]
Type=simple
User=guru
ExecStart=/usr/bin/humanoid-daemon
[Install]
WantedBy=multi-user.target
#!/bin/bash
#
# Humanoidctl
#
# This script can be symlinked in /usr/bin and contains a number of useful ROS2 commands such as:
# humanoid env - source the required overlays (must use 'source' bash command, see below)
# humanoid shell - start a new bash shell with the required overlays
# humanoid dumpenv - dump all the ROS2 relevant environment variables (i.e. for development in CLion or other IDE)
# humanoid launch <name> - launch the <name>.launch.py ROS2 file (also sources overlays if required)
# - also has some specific launch handlers for visualization or other launch scenarios
# humanoid build - call colcon build --symlink-install
# humanoid test - call default build and launch
#
# You should easily be able to modify this command to your liking. You need to at least edit the 'env' sub-routine
# to specify the overlays for your project.
#
# SOURCING OVERLAYS
# When using the 'humanoid env' command in a shell to source your overlays, you must 'source humanoid env' so that bash
# will import the overlays ENV variables into the current shell. If you dont want this, then use the 'humanoid shell'
# command to create a new bash shell with the overlays active. You can then 'exit' to return to your original shell.
#
ROS_HOME=${ROS_HOME:=//opt/ros/eloquent}
HUMANOID_HOME=${HUMANOID_HOME:=/home/guru/lss-humanoid}
cd $HUMANOID_HOME/ros2/humanoid
function humanoid_check_env {
if [ -z "$COLCON_PREFIX_PATH" ]; then
echo "humanoid $humanoid_cmd requires ROS2 environment. Use 'humanoidctl env' or directly source the required ROS2 overlays"
return 2
else
return 0
fi
}
function humanoid_env {
echo "Sourcing ROS overlays"
source $ROS_HOME/install/setup.bash
source $HUMANOID_HOME/ros2/3rdparty/install/local_setup.bash
source $HUMANOID_HOME/ros2/humanoid/install/local_setup.bash
}
function humanoid_require_env {
if [ -z "$COLCON_PREFIX_PATH" ]; then
humanoid_env
fi
}
function humanoid_dumpenv {
ros_env="AMENT_PREFIX_PATH CMAKE_PREFIX_PATH COLCON_PREFIX_PATH PKG_CONFIG_PATH PYTHONPATH LD_LIBRARY_PATH PATH ROS_DISTRO ROS_PYTHON_VERSION ROS_LOCALHOST_ONLY ROS_VERSION"
if humanoid_require_env; then
for e in ${ros_env}; do
echo "$e=${!e}"
done
fi
}
function humanoid_shell {
humanoid_require_env
bash
}
function humanoid_launch {
if humanoid_require_env; then
# can alternatively specify the launch file prefix, ex: 'hardware' to only run the hardware on the RPi
launch_file=${1:-display}
case "${launch_file}" in
desktop)
# launch the desktop file with remote hardware
echo "Starting ROS2 desktop interface"
ros2 launch lss_humanoid display.launch.py hardware:=remote-hardware display:=rviz
;;
jupyter)
# launch jupyter notebook server
source ${HUMANOID_HOME}/jupyter/bin/activate
cd ${HUMANOID_HOME}/notebooks
${HUMANOID_HOME}/jupyter/bin/jupyter-notebook
;;
*)
echo "Starting ROS2 humanoid ($launch_file)"
ros2 launch lss_humanoid ${launch_file}.launch.py
;;
esac
return $?
fi
}
function humanoid_build {
if humanoid_require_env; then
colcon build --symlink-install
return $?
fi
}
function humanoid_test {
humanoid_build && humanoid_launch $*
return $?
}
function humanoid {
if [[ $# -ge 1 ]]; then
humanoid_cmd=humanoid_$1
shift
$humanoid_cmd $*
else
echo 'usage: humanoid env|shell|launch|build|test <args>'
fi
}
if [[ $# -ge 1 ]]; then
humanoid $*
fi
#
# many custom paths in here you will need to modify, but here is the general steps to installing the above files
#
# install binaries to /usr/bin
ln -s ~/lss-humanoid/conf/systemd/humanoid-daemon /usr/bin/humanoid-daemon
ln -s ~/lss-humanoid/conf/humanoidctl /usr/bin/humanoidctl
# install service spec file to systemd
ln -s ~/lss-humanoid/conf/systemd/humanoid.service /etc/systemd/system/humanoid.service
# ensure execute permissions
chmod +x /usr/sbin/humanoid-daemon
chmod +x /usr/sbin/humanoidctl
# import the service into the system
sudo systemctl enable humanoid.service
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment