From 54f503a6e7a839b1359a2cdc0241a1a77621bfab Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sun, 6 Dec 2015 19:27:46 +0100 Subject: [PATCH] added rgb stripes --- CMakeLists.txt | 21 +++++++++++---------- msg/Led.msg | 4 ++++ msg/LedStripe.msg | 1 + package.xml | 2 ++ scripts/christmas.py | 24 ++++++++++++++++++++++++ scripts/wt_node.py | 42 ++++++++++++++++++++++++++++++++++++++++-- 6 files changed, 82 insertions(+), 12 deletions(-) create mode 100644 msg/Led.msg create mode 100644 msg/LedStripe.msg create mode 100755 scripts/christmas.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 5369f92..2e21cc7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(wild_thumper) ## 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 sensor_msgs cv_bridge roscpp std_msgs image_transport dynamic_reconfigure) +find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp std_msgs image_transport dynamic_reconfigure message_generation) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -40,11 +40,11 @@ find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp std_msgs image_transpo ## * 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 -# ) +add_message_files( + FILES + LedStripe.msg + Led.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -61,10 +61,10 @@ find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp std_msgs image_transpo # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs +) generate_dynamic_reconfigure_options( cfg/path_following.cfg @@ -84,6 +84,7 @@ catkin_package( # LIBRARIES wild_thumper # CATKIN_DEPENDS sensor_msgs # DEPENDS system_lib + CATKIN_DEPENDS message_runtime ) ########### diff --git a/msg/Led.msg b/msg/Led.msg new file mode 100644 index 0000000..ec2b24d --- /dev/null +++ b/msg/Led.msg @@ -0,0 +1,4 @@ +int32 num +int32 red +int32 green +int32 blue diff --git a/msg/LedStripe.msg b/msg/LedStripe.msg new file mode 100644 index 0000000..422f680 --- /dev/null +++ b/msg/LedStripe.msg @@ -0,0 +1 @@ +Led[] leds diff --git a/package.xml b/package.xml index 2093ebd..0bf3ca2 100644 --- a/package.xml +++ b/package.xml @@ -46,9 +46,11 @@ std_msgs image_transport openni2_camera + message_generation hector_sensors_description openni2_launch depthimage_to_laserscan + message_runtime diff --git a/scripts/christmas.py b/scripts/christmas.py new file mode 100755 index 0000000..6c53dce --- /dev/null +++ b/scripts/christmas.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import rospy +from random import * +from wild_thumper.msg import LedStripe, Led +from time import sleep + +max_val = 10 + +if __name__ == "__main__": + rospy.init_node('christmas') + pub = rospy.Publisher('led_stripe', LedStripe, queue_size=10) + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + msg = LedStripe() + msg.leds = [ + Led(4, randint(0, max_val), randint(0, max_val), randint(0, max_val)), + Led(5, randint(0, max_val), randint(0, max_val), randint(0, max_val)), + Led(6, randint(0, max_val), randint(0, max_val), randint(0, max_val)), + Led(7, randint(0, max_val), randint(0, max_val), randint(0, max_val)) + ] + pub.publish(msg) + rate.sleep() diff --git a/scripts/wt_node.py b/scripts/wt_node.py index dcae674..96d0db6 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -5,21 +5,50 @@ import rospy import tf import struct import prctl +import spidev from i2c import * from math import * from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from sensor_msgs.msg import Imu, Range +from wild_thumper.msg import LedStripe WHEEL_DIST = 0.248 +class LPD8806: + def __init__(self, bus, device, num_leds): + self.spi = spidev.SpiDev() + self.spi.open(bus, device) + self.spi.mode=0b00 + self.spi.max_speed_hz=int(2e6) + self.num_leds = num_leds + self.latch() + self.l = [(0, 0, 0)] * num_leds + self.update() + + def set(self, i, red=0, green=0, blue=0): + if red > 127 or green > 127 or blue >> 127 or red < 0 or green < 0 or blue < 0: + raise Exception("Bad RGB Value") + self.l[i] = (red, green, blue) + + def latch(self): + self.spi.writebytes([0x0 for i in range((self.num_leds+31)/32)]) + + def update(self): + l = [] + for i in range(self.num_leds): + red, green, blue = self.l[i] + l.append(0x80 | green) + l.append(0x80 | red) + l.append(0x80 | blue) + self.spi.writebytes(l) + self.latch() + class MoveBase: def __init__(self): rospy.init_node('wild_thumper') prctl.set_name("wild_thumper") - rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived) - rospy.Subscriber("imu", Imu, self.imuReceived) enable_odom_tf = rospy.get_param("~enable_odom_tf", True) if enable_odom_tf: self.tf_broadcaster = tf.broadcaster.TransformBroadcaster() @@ -35,6 +64,10 @@ class MoveBase: rospy.loginfo("Init done") i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction self.handicap_last = (-1, -1) + self.pStripe = LPD8806(1, 0, 12) + rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived) + rospy.Subscriber("imu", Imu, self.imuReceived) + rospy.Subscriber("led_stripe", LedStripe, self.led_stripe_received) self.run() def run(self): @@ -246,6 +279,11 @@ class MoveBase: dist = self.read_dist_srf(0x17) self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 40) self.start_dist_srf(0x7) # get next value + + def led_stripe_received(self, msg): + for led in msg.leds: + self.pStripe.set(led.num, red=led.red, green=led.green, blue=led.blue) + self.pStripe.update() if __name__ == "__main__": -- 2.39.2