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.

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

State machine #377

Closed
wants to merge 7 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
The table of contents is too big for display.
Diff view
Diff view
  •  
  •  
  •  
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
project(reactive_system_urc)
project(reactive_system)

# Build in "Release" (with lots of compiler optimizations) by default
# (If built in "Debug", some functions can take orders of magnitude longer)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package format="2">
<name>reactive_system_urc</name>
<name>reactive_system</name>
<version>0.0.0</version>
<description>The reactive_system_urc package</description>
<description>The reactive_system package</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
/**
/**#include <Ros.h>

* Created by William Gu on Sept 29 2018
* Runs the reactive system node
*/
Expand Down
28 changes: 28 additions & 0 deletions src/state_machine_circ/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
cmake_minimum_required(VERSION 2.8.3)
project(state_machine_circ)

find_package(catkin REQUIRED COMPONENTS rospy)

catkin_python_setup()

catkin_package()

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
install(PROGRAMS
"src/state_machine.py"
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
#add_rostest(state_machine_rostest test/state_machine_circ_test.test test/state_machine_rostest.py)
add_rostest(test/state_machine_circ_test.test)
endif()
5 changes: 5 additions & 0 deletions src/state_machine_circ/launch/state_machine.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<!-- A .launch file which launches the node -->

<launch>
<node name="state_machine" pkg="state_machine_circ" type="state_machine.py" output="screen"/>
</launch>
36 changes: 36 additions & 0 deletions src/state_machine_circ/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
<?xml version="1.0"?>
<package format="2">
<name>state_machine_circ</name>
<version>0.0.0</version>
<description>The state_machine_circ package</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
<maintainer email="[email protected]">william</maintainer>


<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>


<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<buildtool_depend>catkin</buildtool_depend>

<build_depend>rospy</build_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>rospy</exec_depend>

<build_depend>smach</build_depend>
<build_export_depend>smach</build_export_depend>
<exec_depend>smach</exec_depend>

<build_depend>smach_ros</build_depend>
<build_export_depend>smach_ros</build_export_depend>
<exec_depend>smach_ros</exec_depend>


</package>
10 changes: 10 additions & 0 deletions src/state_machine_circ/setup.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
#!/usr/bin/env python

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup

d = generate_distutils_setup(
packages=['state_machine_circ'],
package_dir={'': 'src'},
)
setup(**d)
84 changes: 84 additions & 0 deletions src/state_machine_circ/src/StateMachine.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
"""
State Machine Node implementation
Created by William Gu on Feb 23 2019
"""

import rospy
import smach
from std_msgs.msg import String


class StateMachine:

""" Task status: ongoing, idle, next """
""" Various task nodes should output only ongoing and idle """
move_status = 'next' # indicates next expected state
drill_status = 'idle'

state_pub = rospy.Publisher('state', String, queue_size=1) # State publisher

def __init__(self, node_name):
rospy.init_node(node_name)

''' Setup subscribers for state variables '''
rospy.Subscriber('move_status', String, self.moveCallBack)
rospy.Subscriber('drill_status', String, self.drillCallBack)

# Create a SMACH state machine
sm = smach.StateMachine(outcomes=[])

# Open the container
with sm:
# Add states to the container (1st state listed = initial state?)
smach.StateMachine.add('MOVE', Move(),
transitions={'ongoing': 'MOVE',
'done': 'DRILL'})
smach.StateMachine.add('DRILL', Drill(),
transitions={'ongoing': 'DRILL',
'done': 'MOVE'})

# Execute SMACH plan
sm.execute()

""" Call backs """
def moveCallBack(self, msg):
if msg.data == 'ongoing': # Override 'next' status when it starts
StateMachine.move_status = msg.data
elif StateMachine.move_status != 'next': # Otherwise do NOT update if next state
StateMachine.move_status = msg.data

def drillCallBack(self, msg):
if msg.data == 'ongoing':
StateMachine.drill_status = msg.data
elif StateMachine.drill_status != 'next':
StateMachine.drill_status = msg.data


# define state Move
class Move(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['done', 'ongoing'])

def execute(self, userdata):
StateMachine.state_pub.publish('MOVE') # publish current state
if StateMachine.move_status == 'idle':
StateMachine.drill_status = 'next' # Setup next expected status
return 'done'
else: # ongoing or next
return 'ongoing'


# define state Drill
class Drill(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['done', 'ongoing'])

def execute(self, userdata):
StateMachine.state_pub.publish('DRILL')
if StateMachine.drill_status == 'idle':
StateMachine.move_status = 'next'
return 'done'
else:
return 'ongoing'
Binary file added src/state_machine_circ/src/StateMachine.pyc
Binary file not shown.
12 changes: 12 additions & 0 deletions src/state_machine_circ/src/state_machine.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
#!/usr/bin/env python
"""
Entry point for state machine
Created by William Gu on Feb 23 2019
"""
import rospy
import StateMachine

if __name__== "__main__":

hp = StateMachine.StateMachine("StateMachine") # initialize node
rospy.spin()
6 changes: 6 additions & 0 deletions src/state_machine_circ/test/state_machine_circ_test.test
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<!-- A .test file which launches the node and its corresponding rostest -->

<launch>
<node name="state_machine" pkg="state_machine_circ" type="state_machine.py"/>
<test test-name="state_machine_rostest" pkg="state_machine_circ" type="state_machine_rostest.py"/>
</launch>
62 changes: 62 additions & 0 deletions src/state_machine_circ/test/state_machine_rostest.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
#!/usr/bin/env python

# Test dependencies
from std_msgs.msg import String
import time
import sys
import unittest
import rospy
import rostest

PKG = 'state_machine_circ'
NAME = 'state_machine_rostest'

class ROSTest(unittest.TestCase):

def __init__(self, *args):
super(ROSTest, self).__init__(*args)
self.curState = 'default'
self.move_publisher = rospy.Publisher('move_status', String, queue_size=1)
self.drill_publisher = rospy.Publisher('drill_status', String, queue_size=1)

# Callback subscriber for current state of state node
def callback(self, msg):
self.curState = msg.data

# Test for move -> drill -> move state
def test_state_transition(self):
rospy.init_node(NAME, anonymous=True)
rospy.Subscriber('state', String, self.callback)

# Initial Move State
timeout_t = time.time() + 1.0 # 1 second
while not rospy.is_shutdown() and time.time() < timeout_t:
self.move_publisher.publish('ongoing')
time.sleep(0.1)
self.assertEqual(self.curState, 'MOVE')

# Transition to Drill State
self.move_publisher.publish('idle')
time.sleep(0.1) # delay to simulate time to process new state
self.assertEqual(self.curState, 'DRILL')
timeout_t = time.time() + 1.0 # 1 second
while not rospy.is_shutdown() and time.time() < timeout_t:
self.move_publisher.publish('idle')
self.drill_publisher.publish('ongoing')
time.sleep(0.1)
self.assertEqual(self.curState, 'DRILL')

# Transition back to Move State
self.drill_publisher.publish('idle')
time.sleep(0.1) # delay to simulate time to process new state
self.assertEqual(self.curState, 'MOVE')
timeout_t = time.time() + 1.0 # 1 second
while not rospy.is_shutdown() and time.time() < timeout_t:
self.drill_publisher.publish('idle')
self.move_publisher.publish('ongoing')
time.sleep(0.1)
self.assertEqual(self.curState, 'MOVE')


if __name__ == '__main__':
rostest.rosrun(PKG, NAME, ROSTest, sys.argv)
76 changes: 76 additions & 0 deletions src/venv/bin/activate
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
# This file must be used with "source bin/activate" *from bash*
# you cannot run it directly

deactivate () {
# reset old environment variables
if [ -n "${_OLD_VIRTUAL_PATH:-}" ] ; then
PATH="${_OLD_VIRTUAL_PATH:-}"
export PATH
unset _OLD_VIRTUAL_PATH
fi
if [ -n "${_OLD_VIRTUAL_PYTHONHOME:-}" ] ; then
PYTHONHOME="${_OLD_VIRTUAL_PYTHONHOME:-}"
export PYTHONHOME
unset _OLD_VIRTUAL_PYTHONHOME
fi

# This should detect bash and zsh, which have a hash command that must
# be called to get it to forget past commands. Without forgetting
# past commands the $PATH changes we made may not be respected
if [ -n "${BASH:-}" -o -n "${ZSH_VERSION:-}" ] ; then
hash -r
fi

if [ -n "${_OLD_VIRTUAL_PS1:-}" ] ; then
PS1="${_OLD_VIRTUAL_PS1:-}"
export PS1
unset _OLD_VIRTUAL_PS1
fi

unset VIRTUAL_ENV
if [ ! "$1" = "nondestructive" ] ; then
# Self destruct!
unset -f deactivate
fi
}

# unset irrelevant variables
deactivate nondestructive

VIRTUAL_ENV="/home/william/Snowflake/src/venv"
export VIRTUAL_ENV

_OLD_VIRTUAL_PATH="$PATH"
PATH="$VIRTUAL_ENV/bin:$PATH"
export PATH

# unset PYTHONHOME if set
# this will fail if PYTHONHOME is set to the empty string (which is bad anyway)
# could use `if (set -u; : $PYTHONHOME) ;` in bash
if [ -n "${PYTHONHOME:-}" ] ; then
_OLD_VIRTUAL_PYTHONHOME="${PYTHONHOME:-}"
unset PYTHONHOME
fi

if [ -z "${VIRTUAL_ENV_DISABLE_PROMPT:-}" ] ; then
_OLD_VIRTUAL_PS1="${PS1:-}"
if [ "x(venv) " != x ] ; then
PS1="(venv) ${PS1:-}"
else
if [ "`basename \"$VIRTUAL_ENV\"`" = "__" ] ; then
# special case for Aspen magic directories
# see http://www.zetadev.com/software/aspen/
PS1="[`basename \`dirname \"$VIRTUAL_ENV\"\``] $PS1"
else
PS1="(`basename \"$VIRTUAL_ENV\"`)$PS1"
fi
fi
export PS1
fi

# This should detect bash and zsh, which have a hash command that must
# be called to get it to forget past commands. Without forgetting
# past commands the $PATH changes we made may not be respected
if [ -n "${BASH:-}" -o -n "${ZSH_VERSION:-}" ] ; then
hash -r
fi
37 changes: 37 additions & 0 deletions src/venv/bin/activate.csh
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
# This file must be used with "source bin/activate.csh" *from csh*.
# You cannot run it directly.
# Created by Davide Di Blasi <[email protected]>.
# Ported to Python 3.3 venv by Andrew Svetlov <[email protected]>

alias deactivate 'test $?_OLD_VIRTUAL_PATH != 0 && setenv PATH "$_OLD_VIRTUAL_PATH" && unset _OLD_VIRTUAL_PATH; rehash; test $?_OLD_VIRTUAL_PROMPT != 0 && set prompt="$_OLD_VIRTUAL_PROMPT" && unset _OLD_VIRTUAL_PROMPT; unsetenv VIRTUAL_ENV; test "\!:*" != "nondestructive" && unalias deactivate'

# Unset irrelevant variables.
deactivate nondestructive

setenv VIRTUAL_ENV "/home/william/Snowflake/src/venv"

set _OLD_VIRTUAL_PATH="$PATH"
setenv PATH "$VIRTUAL_ENV/bin:$PATH"


set _OLD_VIRTUAL_PROMPT="$prompt"

if (! "$?VIRTUAL_ENV_DISABLE_PROMPT") then
if ("venv" != "") then
set env_name = "venv"
else
if (`basename "VIRTUAL_ENV"` == "__") then
# special case for Aspen magic directories
# see http://www.zetadev.com/software/aspen/
set env_name = `basename \`dirname "$VIRTUAL_ENV"\``
else
set env_name = `basename "$VIRTUAL_ENV"`
endif
endif
set prompt = "[$env_name] $prompt"
unset env_name
endif

alias pydoc python -m pydoc

rehash
Loading