ROS2 Humanoid Control and SystemD

humanoid-daemon

#!/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()

humanoid.service

[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

humanoidctl

#!/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

install.sh

#
#  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