diff --git a/CMakeLists.txt b/CMakeLists.txt
index e9bb7e3..43d8a18 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
+ message_generation
)
## System dependencies are found with CMake's conventions
@@ -47,18 +48,17 @@ find_package(catkin REQUIRED COMPONENTS
## * 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
+ Neopixel.msg
+ Neopixels.msg
+)
## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
+add_service_files(
+ FILES
+ FillNeopixels.srv
+)
## Generate actions in the 'action' folder
# add_action_files(
@@ -68,10 +68,10 @@ find_package(catkin REQUIRED COMPONENTS
# )
## 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
+)
################################################
## Declare ROS dynamic reconfigure parameters ##
@@ -107,6 +107,7 @@ catkin_package(
# LIBRARIES simple_example
# CATKIN_DEPENDS roscpp rospy stdmsgs
# DEPENDS system_lib
+ CATKIN_DEPENDS message_runtime
)
###########
diff --git a/launch/simple.launch b/launch/simple.launch
new file mode 100644
index 0000000..2bc2a73
--- /dev/null
+++ b/launch/simple.launch
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/msg/Neopixel.msg b/msg/Neopixel.msg
new file mode 100644
index 0000000..5f4318c
--- /dev/null
+++ b/msg/Neopixel.msg
@@ -0,0 +1,4 @@
+uint8 r
+uint8 g
+uint8 b
+uint8 w
diff --git a/msg/Neopixels.msg b/msg/Neopixels.msg
new file mode 100644
index 0000000..07a99d4
--- /dev/null
+++ b/msg/Neopixels.msg
@@ -0,0 +1 @@
+Neopixel[] pixels
diff --git a/package.xml b/package.xml
index cc15d36..4b0b48c 100644
--- a/package.xml
+++ b/package.xml
@@ -37,13 +37,13 @@
-
+ message_generation
-
+ message_runtime
diff --git a/scripts/NeopixelDriver.py b/scripts/NeopixelDriver.py
new file mode 100644
index 0000000..7a77ad7
--- /dev/null
+++ b/scripts/NeopixelDriver.py
@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+import rospy
+
+from serial import Serial
+
+from simple_example.msg import Neopixel, Neopixels
+from simple_example.srv import FillNeopixels, FillNeopixelsResponse
+
+
+class NeopixelDriverNode:
+ """ROS Node for controllering NeoPixels."""
+
+ def __init__(self):
+ self._msg = Neopixels()
+ self.pixel_count = rospy.get_param('/simple_example/pixel_count', 1)
+ self.port = rospy.get_param('/simple_example/port', "/dev/ttyUSB0")
+ self.serial = Serial(self.port, 115200, timeout=1)
+
+ def listen(self):
+ rospy.init_node("light_ctrl", anonymous=True)
+ rospy.Subscriber("/neopixel", Neopixels, self.neopixel_ctrl_callback)
+ rate = rospy.spin()
+
+ def neopixel_ctrl_callback(self, data):
+ cmds = ""
+ for i, pixel in data.pixels:
+ cmds += f"{i},{pixel.r},{pixel.g},{pixel.b};"
+ cmds += "\n"
+ rospy.logdebug(f"Sending: {cmds}")
+ self.serial.write(cmds.encode("ascii"))
+
+
+if __name__ == "__main__":
+ rospy.loginfo("Firing up neopixel driver.")
+ node = NeopixelDriverNode()
+ node.listen()
+ rospy.loginfo("Neopixel driver is shutting down.")
diff --git a/scripts/command_node.py b/scripts/command_node.py
index b3bc2a1..9abb8ba 100755
--- a/scripts/command_node.py
+++ b/scripts/command_node.py
@@ -33,7 +33,7 @@ def broadcaster():
global last_command
pub = rospy.Publisher("/command", String, queue_size=10)
- rospy.init_node("keyb_commander", anonymous=True)
+ rospy.init_node("keyb_commander", anonymous=True, log_level=rospy.INFO)
rate = rospy.Rate(10)
print(msg)
@@ -51,7 +51,8 @@ def broadcaster():
if command != last_command and command is not None:
- print("Published command: {}".format(command))
+ # print("Published command: {}".format(command))
+ rospy.loginfo("Published command: {}".format(command))
last_command = command
if command is not None:
diff --git a/scripts/drive_node.py b/scripts/drive_node.py
index 69eac5c..cd8f840 100755
--- a/scripts/drive_node.py
+++ b/scripts/drive_node.py
@@ -20,7 +20,7 @@ def command_callback(message):
COMMAND_MAP.get(received_command, unknown_command)()
if received_command != last_received_command:
- print("Received command: {}".format(received_command))
+ rospy.loginfo("Received command: {}".format(received_command))
last_received_command = received_command
diff --git a/srv/FillNeopixels.srv b/srv/FillNeopixels.srv
new file mode 100644
index 0000000..146c553
--- /dev/null
+++ b/srv/FillNeopixels.srv
@@ -0,0 +1,8 @@
+#request constants
+
+#request fields
+Neopixel pixel
+---
+#response constants
+
+#response fields
\ No newline at end of file