STEVE_Cammast/Steve/Scripts/MastMonitor.py

175 lines
6.7 KiB
Python
Raw Normal View History

2021-04-08 13:04:58 +02:00
#!/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