175 lines
6.7 KiB
Python
175 lines
6.7 KiB
Python
#!/usr/bin/env python
|
|
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Mon Feb 10 17:09:57 2020
|
|
|
|
@author: Maxi
|
|
"""
|
|
|
|
import re
|
|
import roslib
|
|
import rospy
|
|
import sys
|
|
|
|
import std_msgs.msg as msg
|
|
from sensor_msgs.msg import JointState
|
|
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
|
|
|
|
# Import custom message data and dynamic reconfigure variables.
|
|
from Steve.msg import node_example
|
|
from Steve.cfg import node_example_paramsConfig as ConfigType
|
|
|
|
class Monitor:
|
|
def __init__(self):
|
|
self.statusOK = True
|
|
self.counter = 0
|
|
""" status """
|
|
self.rotation = 0
|
|
self.elevation = 'stowed'
|
|
self.elev_angle = 90 # deg
|
|
self.servo = 0
|
|
|
|
""" ROS setup """
|
|
rospy.init_node('Monitor')
|
|
self.pub = rospy.Publisher('monitor',
|
|
msg.String,
|
|
queue_size=5)
|
|
self.InterruptPub = rospy.Publisher('interrupt',
|
|
msg.String,
|
|
queue_size=3)
|
|
self.pub_gs_monitor = rospy.Publisher('groundstation_monitoring',
|
|
JointState,
|
|
queue_size=1)
|
|
self.listener_gs_cmd = rospy.Subscriber('groundstation_command',
|
|
JointState,
|
|
callback=self.callback_cmd)
|
|
self.listener_gs_monitor = rospy.Subscriber('groundstation_monitoring',
|
|
JointState,
|
|
callback=self.callback_mon)
|
|
self.listener_ch = rospy.Subscriber('chatter',
|
|
node_example,
|
|
callback=self.callback_custom_msg)
|
|
self.listener_st = rospy.Subscriber('stowed_switch',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='stowed_switch')
|
|
self.listener_ra = rospy.Subscriber('raised_switch',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='raised_switch')
|
|
self.listener_lim = rospy.Subscriber('limit_switch',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='limit_switch')
|
|
self.listener_test = rospy.Subscriber('tests',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='tests')
|
|
self.listener_int = rospy.Subscriber('interrupt',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='interrupt')
|
|
self.listener_cmd = rospy.Subscriber('command',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='command')
|
|
self.listener_forw = rospy.Subscriber('forward_switch',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='forward_switch')
|
|
self.listener_rot = rospy.Subscriber('rotation',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='rotation')
|
|
self.listener_elev = rospy.Subscriber('elevation',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='elevation')
|
|
self.listener_servo = rospy.Subscriber('servo',
|
|
msg.String,
|
|
callback=self.callback_generic,
|
|
callback_args='servo')
|
|
while not rospy.is_shutdown():
|
|
self.enable_activities()
|
|
rospy.spin()
|
|
# time.sleep(0.1)
|
|
|
|
|
|
|
|
def callback_generic(self, data, topic):
|
|
rospy.loginfo('%s: sent me %s', topic, data.data)
|
|
if topic == 'rotation':
|
|
self.rotation = float(data.data)
|
|
elif topic == 'elevation':
|
|
if 'STATE' in data.data:
|
|
self.elevation = re.sub('STATE: ', '', data.data)
|
|
rospy.loginfo(self.elevation)
|
|
elif 'ANGLE' in data.data:
|
|
self.elev_angle = int(re.findall('-*[0-9]+', data.data)[0])
|
|
rospy.loginfo(self.elev_angle)
|
|
elif topic == 'servo':
|
|
self.servo = float(data.data)
|
|
if self.counter > 10:
|
|
self.enable_activities()
|
|
self.send_status()
|
|
self.counter = 0
|
|
self.counter += 1
|
|
|
|
def callback_custom_msg(self, data):
|
|
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
|
|
data.message)
|
|
|
|
def callback_cmd(self, cmd):
|
|
rospy.loginfo(rospy.get_caller_id() + 'I heard the command \n\t Sequence: %s \n\t FrameID: %s \n\t Joint: %s \n\t Position: %s \n\t Velocity: %s \n\t Effort: %s',
|
|
cmd.header.seq, cmd.header.frame_id, cmd.name, cmd.position, cmd.velocity, cmd.effort)
|
|
|
|
def callback_mon(self, cmd):
|
|
rospy.loginfo(rospy.get_caller_id() + 'Status was sent \n\t Sequence: %s \n\t FrameID: %s \n\t Joint: %s \n\t Position: %s \n\t Velocity: %s \n\t Effort: %s',
|
|
cmd.header.seq, cmd.header.frame_id, cmd.name, cmd.position, cmd.velocity, cmd.effort)
|
|
|
|
def enable_activities(self):
|
|
if abs(self.rotation) < 1.5:
|
|
self.InterruptPub.publish(msg.String('ELEVATE: ENABLE'))
|
|
else:
|
|
self.InterruptPub.publish(msg.String('ELEVATE: DISABLE'))
|
|
|
|
if self.elevation == 'raised':
|
|
self.InterruptPub.publish(msg.String('ROTATE: ENABLE'))
|
|
self.InterruptPub.publish(msg.String('SERVO: ENABLE'))
|
|
else:
|
|
self.InterruptPub.publish(msg.String('ROTATE: DISABLE'))
|
|
self.InterruptPub.publish(msg.String('SERVO: DISABLE'))
|
|
|
|
def send_status(self):
|
|
msg = JointState()
|
|
msg.name = ['J1_turntable', 'J2_fold', 'J3_nod']
|
|
msg.position = [self.deg_to_rad(self.rotation),
|
|
self.deg_to_rad(self.elev_angle),
|
|
self.deg_to_rad(self.servo)]
|
|
msg.velocity = []
|
|
msg.effort = []
|
|
self.pub_gs_monitor.publish(msg)
|
|
|
|
def deg_to_rad(self, degrees):
|
|
rad = degrees * 3.1415 / 180.0
|
|
return rad
|
|
|
|
def elev_to_rad(self, state):
|
|
if state == 'raised':
|
|
return 0.0
|
|
elif state == 'stowed':
|
|
return deg_to_rad(90)
|
|
elif state == 'tilted':
|
|
return deg_to_rad(-30)
|
|
else:
|
|
return
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
monitor = Monitor()
|
|
except rospy.ROSInterruptException:
|
|
pass
|