This commit is contained in:
2015-05-19 03:42:02 +02:00
parent 7156280bae
commit 3ff42cb533
30 changed files with 965 additions and 123 deletions
+169
View File
@@ -0,0 +1,169 @@
cmake_minimum_required(VERSION 2.8.3)
project(hos_dummy)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
hos_core_msgs
hos_python_json
rospy
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# hos_core_msgs
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES hos_dummy
CATKIN_DEPENDS hos_core_msgs hos_python_json rospy
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
## Declare a cpp library
# add_library(hos_dummy
# src/${PROJECT_NAME}/hos_dummy.cpp
# )
## Declare a cpp executable
# add_executable(hos_dummy_node src/hos_dummy_node.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(hos_dummy_node hos_dummy_generate_messages_cpp)
## Specify libraries to link a library or executable target against
# target_link_libraries(hos_dummy_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
#And this will install python nodes
catkin_install_python(PROGRAMS
nodes/hos_dummy_devices.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/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
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS hos_dummy hos_dummy_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_hos_dummy.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
+28
View File
@@ -0,0 +1,28 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import socket
import rospy
import traceback
from hos_core_msgs.msg import DeviceCommand
from hos_dummy.HosDummyController import HosDummyController
import Tkinter as tk
import Tkconstants
if __name__ == '__main__':
try:
root = tk.Tk();
app = HosDummyController(root)
root.geometry("+200+200")
app.master.title('HOS Dummy Controller')
app.mainloop()
except socket.error as e:
print "Could not connect to master"
except Exception as inst:
print type(inst) # the exception instance
print inst # __str__ allows args to be printed directly
traceback.print_exc()
+33
View File
@@ -0,0 +1,33 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import socket
import rospy
import traceback
from hos_core_msgs.msg import DeviceCommand
from hos_dummy.HosDummyDevices import HosDummyDevices
import Tkinter as tk
import Tkconstants
def deviceCmdCallback(data):
app.deviceCmdCallback(data)
if __name__ == '__main__':
try:
root = tk.Tk();
app = HosDummyDevices(root)
root.geometry("+200+200")
app.master.title('HOS Dummy Devices')
rospy.Subscriber(app.cmd_topic, DeviceCommand, deviceCmdCallback)
app.mainloop()
except socket.error as e:
print "Could not connect to master"
except Exception as inst:
print type(inst) # the exception instance
print inst # __str__ allows args to be printed directly
traceback.print_exc()
+56
View File
@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<package>
<name>hos_dummy</name>
<version>0.0.0</version>
<description>The hos_dummy package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="matzman@todo.todo">matzman</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>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/hos_dummy</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>hos_core_msgs</build_depend>
<build_depend>hos_python_json</build_depend>
<build_depend>rospy</build_depend>
<run_depend>hos_core_msgs</run_depend>
<run_depend>hos_python_json</run_depend>
<run_depend>rospy</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
@@ -0,0 +1,144 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import rospy
import json
from hos_pyhue import pyhue
from hos_core_msgs.msg import DeviceCommand
from hos_core_msgs.msg import DeviceStatusUpdate
from hos_core_msgs.msg import HosStatusUpdate
from jsonspec_hos.reference.providers import HOSParamServerProvider
from jsonspec.validators import load
from time import sleep
import Tkinter as tk
import Tkconstants
class HosDummyController(tk.Frame):
def __init__(self, master=None):
# Init ROS node
rospy.init_node('hos_dummy_controller', argv=sys.argv)
# Get ROS parameter
self.status_topic = rospy.get_param('~status_topic', "/hos/devices/status")
self.cmd_topic = rospy.get_param('~cmd_topic', "/hos/devices/cmd")
self.hos_update_topic = rospy.get_param('~hos_update_topic', "/hos/status/updates")
self.loop_frequency = rospy.get_param('~loop_frequency', 100)
self.statusmsg_interval = rospy.get_param('~statusmsg_interval', 6000000)
# Init Variables
self.msg_sequence = 0
# Init devices
self.switches = dict()
self.dimmers = dict()
for i in range(4):
# switches
name = "dummySwitch" + str(i)
self.switches[i] = dict()
self.switches[i]["value"] = False
self.switches[i]["id"] = name
self.switches[i]["name"] = "Switch " + str(i)
key_base = "/hos/devices/repository/" + name
rospy.set_param(key_base + "/type", "core/devices/switch")
rospy.set_param(key_base + "/status_topic", self.status_topic)
rospy.set_param(key_base + "/cmd_topic", self.cmd_topic)
print "Added dummy switch \"" + name + "\""
# dimmers
name = "dummyDimmer" + str(i)
self.dimmers[i] = dict()
self.dimmers[i]["value"] = 0.0
self.dimmers[i]["id"] = name
self.dimmers[i]["name"] = "Dimmer " + str(i)
key_base = "/hos/devices/repository/" + name
rospy.set_param(key_base + "/type", "core/devices/switch")
rospy.set_param(key_base + "/status_topic", self.status_topic)
rospy.set_param(key_base + "/cmd_topic", self.cmd_topic)
print "Added dummy dimmer \"" + name + "\""
# Create JSON schema provider
self.schemaProvider = HOSParamServerProvider()
# Create message publisher
self.commandPublisher = rospy.Publisher(self.cmd_topic, DeviceCommand, queue_size=10)
# Init GUI
tk.Frame.__init__(self, master, padx="10px", pady="10px")
self.grid()
self.createWidgets()
def createWidgets(self):
self.switchframe = tk.LabelFrame(self, text = "Switches", relief=Tkconstants.SUNKEN, bd=1)
self.switchframe.grid(column=0, row=0)
for i in range(4):
self.switches[i]["intvar"] = tk.IntVar(value=int(self.switches[i]["value"]))
self.switches[i]["checkbutton"] = tk.Checkbutton(self.switchframe, text=self.switches[i]["name"], variable = self.switches[i]["intvar"], onvalue = 1, offvalue = 0, height=5, width = 20, command=self.switchCallback)
self.switches[i]["checkbutton"].grid(column=0, row=i)
self.dimmerframe = tk.LabelFrame(self, text = "Dimmers", relief=Tkconstants.SUNKEN, bd=1)
self.dimmerframe.grid(column=1, row=0)
for i in range(4):
self.dimmers[i]["doublevar"] = tk.DoubleVar(value=float(self.dimmers[i]["value"]))
self.dimmers[i]["label"] = tk.Label(self.dimmerframe, text=self.dimmers[i]["name"])
self.dimmers[i]["label"].grid(column=0, row=i)
self.dimmers[i]["scale"] = tk.Scale( self.dimmerframe, variable = self.dimmers[i]["doublevar"], from_=0.0, to=100.0, command=self.scaleCallback)
self.dimmers[i]["scale"].grid(column=1, row=i)
#self.quitButton = tk.Button(self, text='Quit', command=self.quit)
#self.quitButton.grid()
def createSwitchMsg(self, switch, seq, stamp):
msg = DeviceCommand()
msg.seq = seq
msg.device_id = switch["id"]
msg.device_type = "core/devices/switch"
msg.stamp = stamp
msg_body = dict()
msg_body["switch"] = dict()
msg_body["switch"]["set"] = switch["value"]
msg_schema = self.schemaProvider.getTypeSchema("core/devices/switch/command")
validator = load(msg_schema, provider=self.schemaProvider)
validator.validate(msg_body)
msg.msg_body = json.dumps(msg_body)
return msg
def createDimmerMsg(self, dimmer, seq, stamp):
msg = DeviceCommand()
msg.seq = seq
msg.device_id = dimmer["id"]
msg.device_type = "core/devices/dimmer"
msg.stamp = stamp
msg_body = dict()
msg_body["dimmer"] = dict()
msg_body["dimmer"]["set"] = dimmer["value"]/100.0
msg_schema = self.schemaProvider.getTypeSchema("core/devices/dimmer/command")
validator = load(msg_schema, provider=self.schemaProvider)
validator.validate(msg_body)
msg.msg_body = json.dumps(msg_body)
return msg
def scaleCallback(self, newvalue):
stamp = rospy.get_rostime()
for i in self.dimmers:
if (self.dimmers[i]["value"] != self.dimmers[i]["doublevar"].get()):
self.dimmers[i]["value"] = float(self.dimmers[i]["doublevar"].get())
self.commandPublisher.publish(self.createDimmerMsg(self.dimmers[i], self.msg_sequence, stamp))
self.msg_sequence += 1
def switchCallback(self):
stamp = rospy.get_rostime()
for i in self.switches:
if (self.switches[i]["value"] != bool(self.switches[i]["intvar"].get())):
self.switches[i]["value"] = bool(self.switches[i]["intvar"].get())
self.commandPublisher.publish(self.createSwitchMsg(self.switches[i], self.msg_sequence, stamp))
self.msg_sequence += 1
@@ -0,0 +1,206 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import rospy
import json
from hos_pyhue import pyhue
from hos_core_msgs.msg import DeviceCommand
from hos_core_msgs.msg import DeviceStatusUpdate
from hos_core_msgs.msg import HosStatusUpdate
from jsonspec_hos.reference.providers import HOSParamServerProvider
from jsonspec.validators import load
from time import sleep
import Tkinter as tk
import Tkconstants
class HosDummyDevices(tk.Frame):
def __init__(self, master=None):
# Init ROS node
rospy.init_node('hos_dummy_devices', argv=sys.argv)
# Get ROS parameter
self.status_topic = rospy.get_param('~status_topic', "/hos/devices/status")
self.cmd_topic = rospy.get_param('~cmd_topic', "/hos/devices/cmd")
self.hos_update_topic = rospy.get_param('~hos_update_topic', "/hos/status/updates")
self.loop_frequency = rospy.get_param('~loop_frequency', 100)
self.statusmsg_interval = rospy.get_param('~statusmsg_interval', 6000000)
# Init Variables
self.msg_sequence = 0
# Init devices
new_devices = list()
self.switches = dict()
self.dimmers = dict()
for i in range(4):
# switches
name = "dummySwitch" + str(i)
self.switches[i] = dict()
self.switches[i]["value"] = False
self.switches[i]["id"] = name
self.switches[i]["name"] = "Switch " + str(i)
key_base = "/hos/devices/repository/" + name
rospy.set_param(key_base + "/type", "core/devices/switch")
rospy.set_param(key_base + "/status_topic", self.status_topic)
rospy.set_param(key_base + "/cmd_topic", self.cmd_topic)
print "Added dummy switch \"" + name + "\""
new_devices.append(name)
# dimmers
name = "dummyDimmer" + str(i)
self.dimmers[i] = dict()
self.dimmers[i]["value"] = 0.0
self.dimmers[i]["id"] = name
self.dimmers[i]["name"] = "Dimmer " + str(i)
key_base = "/hos/devices/repository/" + name
rospy.set_param(key_base + "/type", "core/devices/switch")
rospy.set_param(key_base + "/status_topic", self.status_topic)
rospy.set_param(key_base + "/cmd_topic", self.cmd_topic)
print "Added dummy dimmer \"" + name + "\""
new_devices.append(name)
# Create JSON schema provider
self.schemaProvider = HOSParamServerProvider()
# Create message publisher
self.statusPublisher = rospy.Publisher(self.status_topic, DeviceStatusUpdate, queue_size=10)
self.hosUpdatePublisher = rospy.Publisher(self.hos_update_topic, HosStatusUpdate, queue_size=10)
# Send HOS Status Update Message
updMsg = HosStatusUpdate()
stamp = rospy.get_rostime()
updMsg.stamp = stamp
updMsg.tags.append("devices")
updBody = dict()
updBody["type"] = "core/msgs/hosstatusupdate"
updBody["devices"] = dict()
updBody["devices"]["type"] = "core/msgs/hosstatusupdate/devices";
updBody["devices"]["added"] = new_devices
msg_schema = self.schemaProvider.getTypeSchema("core/msgs/hosstatusupdate")
validator = load(msg_schema, provider=self.schemaProvider)
validator.validate(updBody)
updMsg.msg_body = json.dumps(updBody)
sleep(1) # Apparently we need to wait before we can send messages
self.hosUpdatePublisher.publish(updMsg)
# Init GUI
tk.Frame.__init__(self, master, padx="10px", pady="10px")
self.grid()
self.createWidgets()
self.deviceStatusCallback()
def createWidgets(self):
self.switchframe = tk.LabelFrame(self, text = "Switches", relief=Tkconstants.SUNKEN, bd=1)
self.switchframe.grid(column=0, row=0)
for i in range(4):
self.switches[i]["stringvar"] = tk.StringVar(value=str(self.switches[i]["value"]))
self.switches[i]["label1"] = tk.Label(self.switchframe, text=self.switches[i]["name"])
self.switches[i]["label1"].grid(column=0, row=i)
self.switches[i]["label2"] = tk.Label(self.switchframe, textvariable=self.switches[i]["stringvar"], relief=Tkconstants.GROOVE, width = 4)
self.switches[i]["label2"].grid(column=1, row=i)
self.dimmerframe = tk.LabelFrame(self, text = "Dimmers", relief=Tkconstants.SUNKEN, bd=1)
self.dimmerframe.grid(column=1, row=0)
for i in range(4):
self.dimmers[i]["stringvar"] = tk.StringVar(value=str(self.dimmers[i]["value"]))
self.dimmers[i]["label1"] = tk.Label(self.dimmerframe, text=self.dimmers[i]["name"])
self.dimmers[i]["label1"].grid(column=0, row=i)
self.dimmers[i]["label2"] = tk.Label(self.dimmerframe, textvariable=self.dimmers[i]["stringvar"], relief=Tkconstants.GROOVE, width = 4)
self.dimmers[i]["label2"].grid(column=1, row=i)
#self.quitButton = tk.Button(self, text='Quit', command=self.quit)
#self.quitButton.grid()
def createSwitchMsg(self, switch, seq, stamp):
msg = DeviceStatusUpdate()
msg.seq = seq
msg.device_id = switch["id"]
msg.device_type = "core/devices/switch"
msg.stamp = stamp
msg_body = dict()
msg_body["switch"] = dict()
msg_body["switch"]["value"] = switch["value"]
msg_schema = self.schemaProvider.getTypeSchema("core/devices/switch/status")
validator = load(msg_schema, provider=self.schemaProvider)
validator.validate(msg_body)
msg.msg_body = json.dumps(msg_body)
return msg
def createDimmerMsg(self, dimmer, seq, stamp):
msg = DeviceStatusUpdate()
msg.seq = seq
msg.device_id = dimmer["id"]
msg.device_type = "core/devices/dimmer"
msg.stamp = stamp
msg_body = dict()
msg_body["dimmer"] = dict()
msg_body["dimmer"]["value"] = dimmer["value"]
msg_schema = self.schemaProvider.getTypeSchema("core/devices/dimmer/status")
validator = load(msg_schema, provider=self.schemaProvider)
validator.validate(msg_body)
msg.msg_body = json.dumps(msg_body)
return msg
def deviceStatusCallback(self):
stamp = rospy.get_rostime()
for switch in self.switches:
self.statusPublisher.publish(self.createSwitchMsg(self.switches[switch], self.msg_sequence, stamp))
for dimmer in self.dimmers:
self.statusPublisher.publish(self.createDimmerMsg(self.dimmers[dimmer], self.msg_sequence, stamp))
self.msg_sequence += 1
self.after(10000, self.deviceStatusCallback)
def deviceCmdCallback(self, data):
rospy.loginfo(rospy.get_caller_id() + ": I heard %s", str(data))
for i in self.switches:
if self.switches[i]["id"] == data.device_id:
self.deviceSwitchCmdCallback(self.switches[i], data)
return
for i in self.dimmers:
if self.dimmers[i]["id"] == data.device_id:
self.deviceDimmerCmdCallback(self.dimmers[i], data)
return
def deviceSwitchCmdCallback(self, switch, data):
msg_body = json.loads(data.msg_body)
msg_schema = self.schemaProvider.getTypeSchema("core/devices/switch/command")
validator = load(msg_schema, provider=self.schemaProvider)
validator.validate(msg_body)
value = msg_body["switch"]["set"]
switch["value"] = bool(value)
switch["stringvar"].set(str(value))
stamp = rospy.get_rostime()
self.statusPublisher.publish(self.createSwitchMsg(switch, self.msg_sequence, stamp))
self.msg_sequence += 1
def deviceDimmerCmdCallback(self, dimmer, data):
print
msg_body = json.loads(data.msg_body)
msg_schema = self.schemaProvider.getTypeSchema("core/devices/dimmer/command")
validator = load(msg_schema, provider=self.schemaProvider)
validator.validate(msg_body)
value = msg_body["dimmer"]["set"]
dimmer["value"] = float(value)
dimmer["stringvar"].set(value)
stamp = rospy.get_rostime()
self.statusPublisher.publish(self.createDimmerMsg(dimmer, self.msg_sequence, stamp))
self.msg_sequence += 1
+11
View File
@@ -0,0 +1,11 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
package_info = generate_distutils_setup(
packages=['hos_dummy'],
package_dir={'': 'python'},
)
setup(**package_info)