-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathled_step_adjust_process_script.py
106 lines (85 loc) · 3.18 KB
/
led_step_adjust_process_script.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
#!/usr/bin/env python
#
# Copyright (c) 2024 Numurus, LLC <https://www.numurus.com>.
#
# This file is part of nepi-engine
# (see https://github.com/nepi-engine).
#
# License: 3-clause BSD, see https://opensource.org/licenses/BSD-3-Clause
#
# Sample NEPI Process Script.
# 1. Waits for LED system
# 2. Steps through LED intensities
import rospy
import time
import sys
from nepi_sdk import nepi_ros
from nepi_sdk import nepi_msg
from std_msgs.msg import Empty, Float32
#########################################
# USER SETTINGS - Edit as Necessary
#########################################
LED_LEVEL_MAX = 0.3 # 0-1 ratio
LED_LEVEL_STEP = 0.05 # 0-1 ratio
LED_STEP_SEC = 1.0
#Set LED Control ROS Topic Name (or partial name)
LED_CONTROL_TOPIC_NAME = "lsx/set_intensity"
#########################################
# Node Class
#########################################
class led_step_adjust(object):
#######################
### Node Initialization
DEFAULT_NODE_NAME = "led_step_adjust" # Can be overwitten by luanch command
def __init__(self):
#### APP NODE INIT SETUP ####
nepi_ros.init_node(name= self.DEFAULT_NODE_NAME)
self.node_name = nepi_ros.get_node_name()
self.base_namespace = nepi_ros.get_base_namespace()
nepi_msg.createMsgPublishers(self)
nepi_msg.publishMsgInfo(self,"Starting Initialization Processes")
##############################
## Initialize Class Variables
self.led_level_max = LED_LEVEL_MAX
self.led_level_step = LED_LEVEL_STEP
self.led_step_sec = LED_STEP_SEC
self.led_last_level = 0
## Define Class Namespaces
## Create Class Publishers
led_control_topic_name = LED_CONTROL_TOPIC_NAME
nepi_msg.publishMsgInfo(self,"Waiting for topic name: " + led_control_topic_name)
led_control_topic=nepi_ros.wait_for_topic(led_control_topic_name)
nepi_msg.publishMsgInfo(self,"Found topic: " + led_control_topic)
self.led_intensity_pub = rospy.Publisher(led_control_topic, Float32, queue_size = 1)
## Start Class Subscribers
## Start Node Processes
#Start level step loop
nepi_msg.publishMsgInfo(self,"Starting LED level step loop")
rospy.Timer(rospy.Duration(self.led_step_sec), self.led_step_callback)
##############################
## Initiation Complete
nepi_msg.publishMsgInfo(self," Initialization Complete")
# Spin forever (until object is detected)
rospy.spin()
##############################
#######################
### Node Methods
### Setup a regular led adjust process
def led_step_callback(self,timer):
led_level = self.led_last_level + self.led_level_step
if led_level > self.led_level_max:
led_level = 0.0
nepi_msg.publishMsgInfo(self,"Setting LED level to: " + '%.2f' % led_level)
if not rospy.is_shutdown():
self.led_intensity_pub.publish(data = led_level)
self.led_last_level = led_level
#######################
# Node Cleanup Function
def cleanup_actions(self):
nepi_msg.publishMsgInfo(self,"Shutting down: Executing script cleanup actions")
self.led_intensity_pub.publish(data = 0)
#########################################
# Main
#########################################
if __name__ == '__main__':
led_step_adjust()