def __init__(self):
rospy.init_node('tank_steer_pd', anonymous=False)
# Grab params
if rospy.has_param('~simulate'):
self.simulate = rospy.get_param('~simulate')
else:
self.simulate = 0
self.max_thrust = rospy.get_param('/max_thrust', 100.0)
self.min_thrust = rospy.get_param('/min_thrust', -100.0)
self.L = rospy.get_param('distance_between_thrusters', 0.6096) #meters
# Set up publishers for servo position and prop speed
self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size = 10)
self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config', DynamixelFullConfig, queue_size = 10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Tank steer PD shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self):
self.force_scale = rospy.get_param("~force_scale", 600)
self.torque_scale = rospy.get_param("~torque_scale", 500)
self.wrench_choices = itertools.cycle(['rc', 'autonomous'])
self.current_pose = Odometry()
self.active = False
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name='kill', action_required=True, severity=0)
# self.docking_alarm = self.alarm_broadcaster.add_alarm(
# name='docking',
# action_required=True,
# severity=0
# )
self.wrench_pub = rospy.Publisher("/wrench/rc",
WrenchStamped,
queue_size=1)
self.kb = KillBroadcaster(id='station_hold', description='Resets Pose')
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy('/change_wrench',
WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
self.reset()
def __init__(self, context):
super(KillPlugin, self).__init__(context)
self.setObjectName('KillPlugin')
self._listener = KillListener()
self._broadcaster = KillBroadcaster(rospy.get_name(),
'Software kill using KillPlugin')
self._kill_active = False
self._widget = QWidget()
loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QTableWidget,
'killTable').setHorizontalHeaderLabels(
["Name", "Status", "Description"])
self._widget.findChild(QPushButton, 'killButton').clicked.connect(
self._on_kill_clicked)
self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(
self._on_unkill_clicked)
self._widget.findChild(QPushButton, 'runButton').clicked.connect(
self._on_run_clicked)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(100)
class wrench_generator:
def __init__(self, name):
# Set up publishers
self.wrench_pub = rospy.Publisher('wrench',
WrenchStamped,
queue_size=10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(
id=name, description='Tank steer wrench_generator shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
# Current pose
self.carrot_position = self.current_position = np.zeros(3)
self.carrot_position = self.current_orientation = np.zeros(3)
self.current_velocity = np.zeros(3)
self.last_perp_velocity = 0
# Grab gains
self.p = rospy.get_param('p', 15)
self.i = rospy.get_param('i', 5)
self.d = rospy.get_param('d', 0)
# Distance before we only orient to goal
self.orientation_radius = rospy.get_param('orientation_radius', 0.5)
# Controller vars
self.last_time = rospy.get_rostime()
# Tf stuff
self.tf_listener = tf.TransformListener()
#self.tf_listener.waitForTransform('/enu', '/base_link', rospy.Time(0), rospy.Time(10000))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_cb,
queue_size=10)
while not self.current_position.any(
): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Start traj subscriber
self.traj_sub = rospy.Subscriber('/trajectory',
PoseTwistStamped,
self.traj_cb,
queue_size=10)
class NetworkCheck(object):
def __init__(self, timeout=5.0, autonomous_msgs_req=50):
self.timeout = rospy.Duration(timeout)
self.last_time = rospy.Time.now()
self.last_msg = ''
# Make sure we don't accidentally let the sub loose.
# We need auto_msgs_req messages before we go autonomous mode.
self.auto_msgs_req = autonomous_msgs_req
self.auto_msg_count = 0
self.sub = rospy.Subscriber('/keep_alive', String, self.got_network_msg, queue_size=1)
self.auto_service = rospy.ServiceProxy('/go_auto', Empty)
self.kb = KillBroadcaster(id='network', description='Network timeout')
#self.alarm_broadcaster, self.alarm = single_alarm('network-timeout', severity=1)
rospy.Timer(rospy.Duration(0.1), self.check)
def check(self, *args):
if self.need_kill() and self.last_msg != '':
if self.auto_msg_count >= self.auto_msgs_req:
rospy.loginfo("AUTONOMOUS MODE STARTED")
self.auto_service()
# Kill the sub after the mission
self.last_msg = 'keep_alive'
self.auto_msg_count = 0
#self.alarm.raise_alarm()
rospy.logerr("KILLING SUB")
self.kb.send(active=True)
else:
self.kb.send(active=False)
#self.alarm.clear_alarm()
def got_network_msg(self, msg):
self.last_msg = msg.data
self.last_time = rospy.Time.now()
if msg.data == 'auto':
if self.auto_msg_count == self.auto_msgs_req:
rospy.loginfo("AUTONOMOUS MODE ARMED")
self.auto_msg_count += 1
else:
self.auto_msg_count = 0
def need_kill(self):
return ((rospy.Time.now() - self.last_time) > self.timeout)
def __init__(self):
self.force_scale = rospy.get_param("~force_scale")
self.torque_scale = rospy.get_param("~torque_scale")
self.last_change_mode = False
self.last_station_hold_state = False
self.last_kill = False
self.last_rc = False
self.last_auto = False
self.killed = False
self.docking = False
self.wrench_choices = itertools.cycle(["rc", "autonomous"])
self.current_pose = Odometry()
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(name="kill", action_required=True, severity=0)
self.docking_alarm = self.alarm_broadcaster.add_alarm(name="docking", action_required=True, severity=0)
self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1)
self.kb = KillBroadcaster(id="station_hold", description="Reset Pose")
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
rospy.Subscriber("odom", Odometry, self.odom_cb)
class wrench_generator:
def __init__(self, name):
# Set up publishers
self.wrench_pub = rospy.Publisher('wrench', WrenchStamped, queue_size = 10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Tank steer wrench_generator shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
# Current pose
self.carrot_position = self.current_position = np.zeros(3)
self.carrot_position = self.current_orientation = np.zeros(3)
self.current_velocity = np.zeros(3)
self.last_perp_velocity = 0
# Grab gains
self.p = rospy.get_param('p', 15)
self.i = rospy.get_param('i', 5)
self.d = rospy.get_param('d', 0)
# Distance before we only orient to goal
self.orientation_radius = rospy.get_param('orientation_radius', 0.5)
# Controller vars
self.last_time = rospy.get_rostime()
# Tf stuff
self.tf_listener = tf.TransformListener()
#self.tf_listener.waitForTransform('/enu', '/base_link', rospy.Time(0), rospy.Time(10000))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Start traj subscriber
self.traj_sub = rospy.Subscriber('/trajectory', PoseTwistStamped, self.traj_cb, queue_size = 10)
def __init__(self, name):
# Set up publishers
self.wrench_pub = rospy.Publisher('wrench',
WrenchStamped,
queue_size=10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(
id=name, description='Tank steer wrench_generator shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self, name):
# Implement the moveto action server
self.moveto_as = actionlib.SimpleActionServer('moveto', MoveToAction, None, False)
# Thread Locking
self.as_lock = self.moveto_as.action_server.lock
self.moveto_as.register_goal_callback(self.new_goal)
self.moveto_as.register_preempt_callback(self.goal_preempt)
# Trajectory generator (using stratagy design pattern where traj_gen is the stratagy)
self.traj_gen = None
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 1.25)
self.angular_tolerance = rospy.get_param('angular_tolerance', 20 * np.pi / 180)
# Publishers
self.traj_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size = 10)
self.traj_debug_pub = rospy.Publisher('/trajectory_debug', PoseStamped, queue_size = 10)
self.goal_debug_pub = rospy.Publisher('/goal_debug', PoseStamped, queue_size = 10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): pass # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Azi waypoint shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
class Handler(object):
alarm_name = 'kill'
def __init__(self):
# Keep some knowledge of which thrusters we have working
self.kb = KillBroadcaster(id='alarm-kill', description='Kill by alarm')
self.alarms = {}
def handle(self, time_sent, parameters, alarm_name):
self.alarms[alarm_name] = True
self.kb.send(active=True)
def cancel(self, time_sent, parameters, alarm_name):
self.alarms[alarm_name] = False
# Make sure that ALL alarms that caused a kill have been cleared
if not any(self.alarms.values()):
self.kb.send(active=False)
def start():
global kill_listener
global kill_broadcaster
#Init node
rospy.init_node('remote_control')
# Register xbox_rc as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
register_controller_proxy('xbox_rc')
# Request to switch to xbox_rc
request_controller_proxy('xbox_rc')
kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Remote control kill')
kill_listener = KillListener(killed_cb, unkilled_cb)
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def thrusterCtrl():
global port_current
global starboard_current
global kill_listener
global kill_broadcaster
#Setup ros
rospy.init_node('thruster_control')
rospy.Subscriber("thruster_config", thrusterNewtons, motorConfigCallback)
r = rospy.Rate(UPDATE_RATE) #1000 hz(1ms Period)... I think
pub_timer = rospy.Timer(PUB_RATE, pubStatus)
# Initilize kill
kill_listener = KillListener(set_kill, clear_kill)
kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Newtons to PWM shutdown')
# Clear in case it was previously killed
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self, name):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 0.5)
self.angular_tolerance = rospy.get_param('angular_tolerance', np.pi / 10)
self.orientation_radius = rospy.get_param('orientation_radius', 0.75)
self.slow_down_radius = rospy.get_param('slow_down_radius', 5)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance', 5)
self.min_tracking_distance = rospy.get_param('min_tracking_distance', 1)
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv', 1)
self.tracking_slope = (self.max_tracking_distance - self.min_tracking_distance) / (self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.tracking_slope * self.orientation_radius + self.min_tracking_distance
# Publishers
self.traj_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size = 10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Initilize a line
self.line = line(np.zeros(3), np.ones(3))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position)
self.redraw_line = False
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Tank steer trajectory_generator shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def thrusterCtrl():
global port_current
global starboard_current
global kill_listener
global kill_broadcaster
global control_kill
#Setup ros
rospy.init_node('thruster_control')
#rospy.Subscriber("control_arbiter", Bool, control_callback)
rospy.Subscriber("thruster_config", thrusterNewtons, motorConfigCallback)
r = rospy.Rate(UPDATE_RATE) #1000 hz(1ms Period)... I think
pub_timer = rospy.Timer(PUB_RATE, pubStatus)
# Initilize kill
kill_listener = KillListener(set_kill, clear_kill)
kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Newtons to PWM shutdown')
# Clear in case it was previously killed
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self, rate=100):
'''I can't swim on account of my ass-mar'''
# Register azi_drive as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
rospy.ServiceProxy('/controller/register_controller',
register_controller)('azi_drive')
self.rate = rate
self.servo_max_rotation = 6.0
self.controller_max_rotation = self.servo_max_rotation / self.rate
# rospy.init_node('azi_drive', log_level=rospy.WARN)
# Set up kill handling
self.kill_listener = KillListener(self.on_kill, self.on_unkill)
self.kill_broadcaster = KillBroadcaster(
id='Azi_drive', description='Azi drive node shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self, name):
# Set up publishers
self.wrench_pub = rospy.Publisher('wrench', WrenchStamped, queue_size = 10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Tank steer wrench_generator shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self, timeout=5.0, autonomous_msgs_req=50):
self.timeout = rospy.Duration(timeout)
self.last_time = rospy.Time.now()
self.last_msg = ''
# Make sure we don't accidentally let the sub loose.
# We need auto_msgs_req messages before we go autonomous mode.
self.auto_msgs_req = autonomous_msgs_req
self.auto_msg_count = 0
self.sub = rospy.Subscriber('/keep_alive', String, self.got_network_msg, queue_size=1)
self.auto_service = rospy.ServiceProxy('/go_auto', Empty)
self.kb = KillBroadcaster(id='network', description='Network timeout')
#self.alarm_broadcaster, self.alarm = single_alarm('network-timeout', severity=1)
rospy.Timer(rospy.Duration(0.1), self.check)
def __init__(self, rate=100):
'''I can't swim on account of my ass-mar'''
# Register azi_drive as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
rospy.ServiceProxy('/controller/register_controller', register_controller)('azi_drive')
self.rate = rate
self.servo_max_rotation = 6.0
self.controller_max_rotation = self.servo_max_rotation / self.rate
# rospy.init_node('azi_drive', log_level=rospy.WARN)
# Set up kill handling
self.kill_listener = KillListener(self.on_kill, self.on_unkill)
self.kill_broadcaster = KillBroadcaster(id='Azi_drive', description='Azi drive node shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self, context):
super(KillPlugin, self).__init__(context)
self.setObjectName('KillPlugin')
self._listener = KillListener()
self._broadcaster = KillBroadcaster(rospy.get_name(), 'Software kill using KillPlugin')
self._kill_active = False
self._widget = QWidget()
loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QTableWidget, 'killTable').setHorizontalHeaderLabels(["Name", "Status", "Description"])
self._widget.findChild(QPushButton, 'killButton').clicked.connect(self._on_kill_clicked)
self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(self._on_unkill_clicked)
self._widget.findChild(QPushButton, 'runButton').clicked.connect(self._on_run_clicked)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(100)
class azi_waypoint:
def __init__(self, name):
# Implement the moveto action server
self.moveto_as = actionlib.SimpleActionServer('moveto', MoveToAction, None, False)
# Thread Locking
self.as_lock = self.moveto_as.action_server.lock
self.moveto_as.register_goal_callback(self.new_goal)
self.moveto_as.register_preempt_callback(self.goal_preempt)
# Trajectory generator (using stratagy design pattern where traj_gen is the stratagy)
self.traj_gen = None
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 1.25)
self.angular_tolerance = rospy.get_param('angular_tolerance', 20 * np.pi / 180)
# Publishers
self.traj_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size = 10)
self.traj_debug_pub = rospy.Publisher('/trajectory_debug', PoseStamped, queue_size = 10)
self.goal_debug_pub = rospy.Publisher('/goal_debug', PoseStamped, queue_size = 10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): pass # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Azi waypoint shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
rospy.on_shutdown(self.on_shutdown)
# Trajectory mode dictionary
self.modes ={
trajectory_modeRequest.POINT_SHOOT_PP: point_shoot_pp,
trajectory_modeRequest.POINT_SHOOT_2_PP: point_shoot_2_pp,
trajectory_modeRequest.A_STAR_RPP: a_star_rpp,
trajectory_modeRequest.GOAL_PP: goal_pp
}
# Initilize Trajectory generator with current position as goal
self.mode = trajectory_modeRequest.POINT_SHOOT_2_PP
self.traj_gen = point_shoot_2_pp()
self.traj_gen.start(self.get_current_posetwist())
# Initilize the switch modes service
mode_srv = rospy.Service('azi_waypoint_mode', trajectory_mode, self.switch_mode)
# Start the main update loop
rospy.Timer(rospy.Duration(0.1), self.update)
self.moveto_as.start()
class KillPlugin(Plugin):
def __init__(self, context):
super(KillPlugin, self).__init__(context)
self.setObjectName('KillPlugin')
self._listener = KillListener()
self._broadcaster = KillBroadcaster(rospy.get_name(),
'Software kill using KillPlugin')
self._kill_active = False
self._widget = QWidget()
loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QTableWidget,
'killTable').setHorizontalHeaderLabels(
["Name", "Status", "Description"])
self._widget.findChild(QPushButton, 'killButton').clicked.connect(
self._on_kill_clicked)
self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(
self._on_unkill_clicked)
self._widget.findChild(QPushButton, 'runButton').clicked.connect(
self._on_run_clicked)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(100)
def _on_update(self):
self._update_kills()
self._update_kill()
def _on_kill_clicked(self):
self._kill_active = True
self._broadcaster.send(True)
self._update_kill()
def _on_unkill_clicked(self):
self._kill_active = False
self._broadcaster.send(False)
self._update_kill()
def _on_run_clicked(self):
self._on_unkill_clicked()
def _update_kills(self):
all_kills = self._listener.get_all_kills()
if all_kills is not None:
new_kills = {
kill.id: kill
for kill in self._listener.get_all_kills()
}
else:
new_kills = {}
table = self._widget.findChild(QTableWidget, 'killTable')
row = 0
while row < table.rowCount():
id = table.item(row, 1)
if id in new_kills:
self._update_kill_entry(row, new_kills[id])
del new_kills[id]
row += 1
else:
table.removeRow(row)
for kill in new_kills.values():
row = table.rowCount()
table.setRowCount(row + 1)
self._update_kill_entry(row, kill)
def _update_kill_entry(self, row, kill):
color = QColor(255, 255, 255) if not kill.active else QColor(
255, 200, 200)
self._update_item(row, 0, color, kill.id)
self._update_item(row, 1, color,
"Killed" if kill.active else "Unkilled")
self._update_item(row, 2, color, kill.description)
def _update_item(self, row, col, color, string):
item = QTableWidgetItem(string)
item.setBackground(color)
self._widget.findChild(QTableWidget,
'killTable').setItem(row, col, item)
def _update_kill(self):
kills = self._listener.get_all_kills()
if kills is not None:
other_kill_count = len([
kill for kill in kills
if kill.id != rospy.get_name() and kill.active
])
self._widget.findChild(
QPushButton, 'runButton').setVisible(other_kill_count == 0)
self._widget.findChild(
QPushButton, 'unkillButton').setVisible(other_kill_count > 0)
status = 'Sub status: '
if not self._listener.get_killed():
status += '<span style="color:green">Running</span>'
else:
status += '<span style="color:red">Killed</span>'
self._widget.findChild(QLabel, 'killStatusLabel').setText(status)
else:
self._widget.findChild(QPushButton, 'runButton').setVisible(False)
self._widget.findChild(QPushButton,
'unkillButton').setVisible(False)
self._widget.findChild(QLabel, 'killStatusLabel').setText(
'<span style="color:red">No kill information</span>')
class KillPlugin(Plugin):
def __init__(self, context):
super(KillPlugin, self).__init__(context)
self.setObjectName('KillPlugin')
self._listener = KillListener()
self._broadcaster = KillBroadcaster(rospy.get_name(), 'Software kill using KillPlugin')
self._kill_active = False
self._widget = QWidget()
loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QTableWidget, 'killTable').setHorizontalHeaderLabels(["Name", "Status", "Description"])
self._widget.findChild(QPushButton, 'killButton').clicked.connect(self._on_kill_clicked)
self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(self._on_unkill_clicked)
self._widget.findChild(QPushButton, 'runButton').clicked.connect(self._on_run_clicked)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(100)
def _on_update(self):
self._update_kills()
self._update_kill()
def _on_kill_clicked(self):
self._kill_active = True
self._broadcaster.send(True)
self._update_kill()
def _on_unkill_clicked(self):
self._kill_active = False
self._broadcaster.send(False)
self._update_kill()
def _on_run_clicked(self):
self._on_unkill_clicked()
def _update_kills(self):
all_kills = self._listener.get_all_kills()
if all_kills is not None:
new_kills = {kill.id: kill for kill in self._listener.get_all_kills()}
else:
new_kills = {}
table = self._widget.findChild(QTableWidget, 'killTable')
row = 0
while row < table.rowCount():
id = table.item(row, 1)
if id in new_kills:
self._update_kill_entry(row, new_kills[id])
del new_kills[id]
row += 1
else:
table.removeRow(row)
for kill in new_kills.values():
row = table.rowCount()
table.setRowCount(row+1)
self._update_kill_entry(row, kill)
def _update_kill_entry(self, row, kill):
color = QColor(255, 255, 255) if not kill.active else QColor(255, 200, 200)
self._update_item(row, 0, color, kill.id)
self._update_item(row, 1, color, "Killed" if kill.active else "Unkilled")
self._update_item(row, 2, color, kill.description)
def _update_item(self, row, col, color, string):
item = QTableWidgetItem(string)
item.setBackground(color)
self._widget.findChild(QTableWidget, 'killTable').setItem(row, col, item)
def _update_kill(self):
kills = self._listener.get_all_kills()
if kills is not None:
other_kill_count = len([kill for kill in kills
if kill.id != rospy.get_name() and kill.active])
self._widget.findChild(QPushButton, 'runButton').setVisible(other_kill_count == 0)
self._widget.findChild(QPushButton, 'unkillButton').setVisible(other_kill_count > 0)
status = 'Sub status: '
if not self._listener.get_killed():
status += '<span style="color:green">Running</span>'
else:
status += '<span style="color:red">Killed</span>'
self._widget.findChild(QLabel, 'killStatusLabel').setText(status)
else:
self._widget.findChild(QPushButton, 'runButton').setVisible(False)
self._widget.findChild(QPushButton, 'unkillButton').setVisible(False)
self._widget.findChild(QLabel, 'killStatusLabel').setText('<span style="color:red">No kill information</span>')
def __init__(self):
# Initilize ros
rospy.init_node("control_arbiter")
# Vars
self.controller = "none"
self.controllers = []
self.floating = False
self.killed = False
self.continuous_angle_lock = threading.Lock()
self.joint_lock = threading.Lock()
self.full_lock = threading.Lock()
self.wheel_lock = threading.Lock()
self.thrust_lock = threading.Lock()
# Services
self.request_controller_srv = rospy.Service(
"request_controller", request_controller, self.request_controller_cb
)
self.register_controller_srv = rospy.Service(
"register_controller", register_controller, self.register_controller_cb
)
self.float_srv = rospy.Service("float_mode", FloatMode, self.set_float_mode)
self.get_all_controllers_srv = rospy.Service(
"get_all_controllers", get_all_controllers, self.get_all_controllers_cb
)
# Publishers
self.continuous_angle_pub = rospy.Publisher(
"/dynamixel/dynamixel_continuous_angle_config", DynamixelContinuousAngleConfig, queue_size=10
)
self.full_pub = rospy.Publisher("/dynamixel/dynamixel_full_config", DynamixelFullConfig, queue_size=10)
self.joint_pub = rospy.Publisher("/dynamixel/dynamixel_joint_config", DynamixelJointConfig, queue_size=10)
self.wheel_pub = rospy.Publisher("/dynamixel/dynamixel_wheel_config", DynamixelWheelConfig, queue_size=10)
self.port_pub = rospy.Publisher("/stm32f3discovery_imu_driver/pwm1", Float64, queue_size=10)
self.starboard_pub = rospy.Publisher("/stm32f3discovery_imu_driver/pwm2", Float64, queue_size=10)
self.float_status_pub = rospy.Publisher("float_status", Bool, queue_size=1)
self.current_controller_pub = rospy.Publisher("current_controller", String, queue_size=1)
# Subscribers
self.continuous_angle_sub = rospy.Subscriber(
"dynamixel_continuous_angle_config",
ControlDynamixelContinuousAngleConfig,
self.continuous_angle_cb,
queue_size=10,
)
self.full_sub = rospy.Subscriber(
"dynamixel_full_config", ControlDynamixelFullConfig, self.full_cb, queue_size=10
)
self.joint_sub = rospy.Subscriber(
"dynamixel_joint_config", ControlDynamixelJointConfig, self.joint_cb, queue_size=10
)
self.wheel_sub = rospy.Subscriber(
"dynamixel_wheel_config", ControlDynamixelWheelConfig, self.wheel_cb, queue_size=10
)
self.thruster_sub = rospy.Subscriber("thruster_config", ControlThrustConfig, self.thruster_cb, queue_size=10)
# Timers
self.update_timer = rospy.Timer(rospy.Duration(0.1), self.status_update)
# Kill
self.kill_listener = KillListener(self.set_kill_cb, self.clear_kill_cb)
self.kill_broadcaster = KillBroadcaster(id="control_arbiter", description="control_arbiter has shutdown")
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
class Node(object):
# Constructor
def __init__(self):
rospy.init_node('tank_steer_pd', anonymous=False)
# Grab params
if rospy.has_param('~simulate'):
self.simulate = rospy.get_param('~simulate')
else:
self.simulate = 0
self.max_thrust = rospy.get_param('/max_thrust', 100.0)
self.min_thrust = rospy.get_param('/min_thrust', -100.0)
self.L = rospy.get_param('distance_between_thrusters', 0.6096) #meters
# Set up publishers for servo position and prop speed
self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size = 10)
self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config', DynamixelFullConfig, queue_size = 10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Tank steer PD shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
# Get subscriber to wrench topic (from trajectory generator)
self.wrench_sub = rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size = 10)
# Setup some constants
# Define thrustors and there position
# TODO: should be retrieved and not hard coded
"""
self.thrusters = [
dict( # port
id=3,
pwm_pub= rospy.Publisher('stm32f3discovery_imu_driver/pwm1', Float64, queue_size = 10),
angle_pub= rospy.Publisher('port_angle', Float64, queue_size = 10),
),
dict( # starboard
id=2,
position= np.array([-.5239, -.3048, -.5]),
angle= 0,
#thrust_range= (min(calib_data[0]), max(calib_data[0])),
dangle_range= (-3.2e-0, +3.2e-0),
angle_range= (-math.pi*.75, +math.pi*.75), # at most (-6*math.pi, +6*math.pi)
effort= 0,
dangle= 0,
pwm_pub= rospy.Publisher('stm32f3discovery_imu_driver/pwm2', Float64, queue_size = 10),
angle_pub= rospy.Publisher('starboard_angle', Float64, queue_size = 10),
),
]
"""
# See Wrench Callback for deffinition of wrench_tf
self.wrench_tf = np.matrix([[0.5, -1 / self.L], [0.5, 1 / self.L]])
# Wait for the dynamixel node to initilze
if not self.simulate:
rospy.loginfo('Checking/waiting for dynamixel server')
while(self.servo_pub.get_num_connections() <= 0 and not rospy.is_shutdown):
pass
rospy.loginfo('dynamixel server found')
# Zero servos
for x in range(2,4):
self.servo_pub.publish(DynamixelFullConfig(
id= x,
goal_position= math.pi,
moving_speed= 12, # near maximum, not actually achievable ...
torque_limit= 1023,
goal_acceleration= 38,
control_mode= DynamixelFullConfig.JOINT,
goal_velocity= 10,
))
# Wait for wrench msgs
rospy.spin()
s = serial.Serial('/dev/serial/by-id/usb-1a86_USB2.0-Serial-if00-port0',baudrate = 9600)
def ReadTerminal():
global s
data = s.readline()
return data
if __name__ == '__main__':
rospy.init_node('kill_switch')
kill_broadcaster = KillBroadcaster(rospy.get_name(),"kill switch kill")
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
kill_switch_pub = rospy.Publisher('kill_switch', String, queue_size = 10)
while not rospy.is_shutdown():
global status
def __init__(self, contex):
super(PropaGatorGUI, self).__init__(contex)
# Initilize variables
self._float_status = False
self.last_odom_msg = Odometry()
self._current_controller = 'Controller: Unknown'
self._controller = ''
self._controllers = []
# Assign a name
self.setObjectName('PropaGatorGUI')
# Create a widget
self._widget = QWidget()
loadUi(os.path.join(cwd, 'propagatorgui.ui'), self._widget)
self._widget.setObjectName('PropaGatorGUI')
contex.add_widget(self._widget)
# Grab all the children from the widget
self._kill_label = self._widget.findChild(QLabel, 'kill_label')
self._float_label = self._widget.findChild(QLabel, 'float_label')
self._controller_label = self._widget.findChild(
QLabel, 'controller_label')
self._odom_x_label = self._widget.findChild(QLabel, 'odom_x_label')
self._odom_y_label = self._widget.findChild(QLabel, 'odom_y_label')
self._odom_yaw_label = self._widget.findChild(QLabel, 'odom_yaw_label')
self._odom_d_x_label = self._widget.findChild(QLabel, 'odom_d_x_label')
self._odom_d_y_label = self._widget.findChild(QLabel, 'odom_d_y_label')
self._odom_d_yaw_label = self._widget.findChild(
QLabel, 'odom_d_yaw_label')
self._placeholder_label = self._widget.findChild(
QLabel, 'placeholder_label')
self._kill_push_btn = self._widget.findChild(QPushButton,
'kill_push_btn')
self._float_push_btn = self._widget.findChild(QPushButton,
'float_push_btn')
self._controller_combo_box = self._widget.findChild(
QComboBox, 'controller_combo_box')
# Load images
self._green_indicator = QPixmap(
os.path.join(cwd, 'green_indicator.png'))
self._red_indicator = QPixmap(os.path.join(cwd, 'red_indicator.png'))
self._placeholder_image = QPixmap(os.path.join(cwd, 'placeholder.png'))
# Set up ROS interfaces
self._kill_listener = KillListener()
self._kill_broadcaster = KillBroadcaster(
id='PropaGator GUI', description='PropaGator GUI kill')
self._odom_sub = rospy.Subscriber('/odom', Odometry, self._odom_cb)
self._float_sub = rospy.Subscriber('/controller/float_status', Bool,
self._float_cb)
self._float_proxy = rospy.ServiceProxy('/controller/float_mode',
FloatMode)
self._current_controller_sub = rospy.Subscriber(
'/controller/current_controller', String,
self._current_controller_cb)
self._get_all_controllers_proxy = rospy.ServiceProxy(
'/controller/get_all_controllers', get_all_controllers)
self._request_controller_proxy = rospy.ServiceProxy(
'/controller/request_controller', request_controller)
# Connect push buttons
self._kill_push_btn.toggled.connect(self._on_kill_push_btn_toggle)
self._float_push_btn.toggled.connect(self._on_float_push_btn_toggle)
# Connect combo boxes
self._controller_combo_box.activated[str].connect(
self._controller_combo_box_cb)
# Set up update timer at 10Hz
# A Qt timer is used instead of a ros timer since Qt components are updated
self.update_timer = QTimer()
self.update_timer.timeout.connect(self._onUpdate)
self.update_timer.start(100)
# Temp
self._placeholder_label.setPixmap(self._placeholder_image)
class Controller(object):
def __init__(self, rate=100):
'''I can't swim on account of my ass-mar'''
# Register azi_drive as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
rospy.ServiceProxy('/controller/register_controller',
register_controller)('azi_drive')
self.rate = rate
self.servo_max_rotation = 6.0
self.controller_max_rotation = self.servo_max_rotation / self.rate
# rospy.init_node('azi_drive', log_level=rospy.WARN)
# Set up kill handling
self.kill_listener = KillListener(self.on_kill, self.on_unkill)
self.kill_broadcaster = KillBroadcaster(
id='Azi_drive', description='Azi drive node shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
self.killed = False
rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(
self.controller_max_rotation))
Azi_Drive.set_delta_alpha_max(self.controller_max_rotation)
#Azi_Drive.set_thrust_bound((-50,50))
# These should not be queued! Old commands are garbage.
# Unfortunately, we have to queue these, because the subscriber cannot process two sequential
# thrust messages as quickly as they are sent
self.thrust_pub = rospy.Publisher('thruster_config',
thrusterNewtons,
queue_size=4)
self.servo_pub = rospy.Publisher('controller/dynamixel_full_config',
ControlDynamixelFullConfig,
queue_size=4)
self.next_wrench = WrenchStamped().wrench
rospy.Subscriber('wrench',
WrenchStamped,
self._wrench_cb,
queue_size=1)
# Thrust topic id's for each thruster
self.left_id = 3
self.right_id = 2
# Time between messages before azi_drive shuts off
# self.control_timeout = 2 # secs
self.control_timeout = np.inf
self.default_angles = np.array([0.0, 0.0], dtype=np.float32)
self.default_forces = np.array([0.0, 0.0], dtype=np.float32)
self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64)
# Left, Right
self.cur_angles = self.default_angles[:]
self.cur_forces = self.default_forces[:]
self.prev_angles = None
self.set_servo_angles(self.cur_angles)
self.set_forces(self.cur_forces)
# Initializations
self.last_msg_time = time()
self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0
self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors
self.MIN_NEWTONS = -100.0 #Full reverse Jacksons
self.ABS_MAX_PW = .002
self.ABS_MIN_PW = .001
self.ZERO_NEWTONS = 0
self.REV_CONV = (self.ZERO_NEWTONS -
self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW)
self.FWD_CONV = (self.MAX_NEWTONS -
self.ZERO_NEWTONS) / self.ABS_MAX_PW
def __init__(self, name):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 0.5)
self.angular_tolerance = rospy.get_param('angular_tolerance',
np.pi / 10)
self.orientation_radius = rospy.get_param('orientation_radius', 0.75)
self.slow_down_radius = rospy.get_param('slow_down_radius', 5)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance',
5)
self.min_tracking_distance = rospy.get_param('min_tracking_distance',
1)
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv',
1)
self.tracking_slope = (
self.max_tracking_distance - self.min_tracking_distance) / (
self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.tracking_slope * self.orientation_radius + self.min_tracking_distance
# Publishers
self.traj_pub = rospy.Publisher('/trajectory',
PoseTwistStamped,
queue_size=10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Initilize a line
self.line = line(np.zeros(3), np.ones(3))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_cb,
queue_size=10)
while not self.current_position.any(
): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(
self.current_position,
tools.normal_vector_from_rotvec(self.current_orientation) +
self.current_position)
self.redraw_line = False
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(
id=name, description='Tank steer trajectory_generator shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def __init__(self):
# Keep some knowledge of which thrusters we have working
self.kb = KillBroadcaster(id='alarm-kill', description='Kill by alarm')
self.alarms = {}
class PropaGatorGUI(Plugin):
def __init__(self, contex):
super(PropaGatorGUI, self).__init__(contex)
# Initilize variables
self._float_status = False
self.last_odom_msg = Odometry()
self._current_controller = 'Controller: Unknown'
self._controller = ''
self._controllers = []
# Assign a name
self.setObjectName('PropaGatorGUI')
# Create a widget
self._widget = QWidget()
loadUi(os.path.join(cwd, 'propagatorgui.ui'), self._widget)
self._widget.setObjectName('PropaGatorGUI')
contex.add_widget(self._widget)
# Grab all the children from the widget
self._kill_label = self._widget.findChild(QLabel, 'kill_label')
self._float_label = self._widget.findChild(QLabel, 'float_label')
self._controller_label = self._widget.findChild(
QLabel, 'controller_label')
self._odom_x_label = self._widget.findChild(QLabel, 'odom_x_label')
self._odom_y_label = self._widget.findChild(QLabel, 'odom_y_label')
self._odom_yaw_label = self._widget.findChild(QLabel, 'odom_yaw_label')
self._odom_d_x_label = self._widget.findChild(QLabel, 'odom_d_x_label')
self._odom_d_y_label = self._widget.findChild(QLabel, 'odom_d_y_label')
self._odom_d_yaw_label = self._widget.findChild(
QLabel, 'odom_d_yaw_label')
self._placeholder_label = self._widget.findChild(
QLabel, 'placeholder_label')
self._kill_push_btn = self._widget.findChild(QPushButton,
'kill_push_btn')
self._float_push_btn = self._widget.findChild(QPushButton,
'float_push_btn')
self._controller_combo_box = self._widget.findChild(
QComboBox, 'controller_combo_box')
# Load images
self._green_indicator = QPixmap(
os.path.join(cwd, 'green_indicator.png'))
self._red_indicator = QPixmap(os.path.join(cwd, 'red_indicator.png'))
self._placeholder_image = QPixmap(os.path.join(cwd, 'placeholder.png'))
# Set up ROS interfaces
self._kill_listener = KillListener()
self._kill_broadcaster = KillBroadcaster(
id='PropaGator GUI', description='PropaGator GUI kill')
self._odom_sub = rospy.Subscriber('/odom', Odometry, self._odom_cb)
self._float_sub = rospy.Subscriber('/controller/float_status', Bool,
self._float_cb)
self._float_proxy = rospy.ServiceProxy('/controller/float_mode',
FloatMode)
self._current_controller_sub = rospy.Subscriber(
'/controller/current_controller', String,
self._current_controller_cb)
self._get_all_controllers_proxy = rospy.ServiceProxy(
'/controller/get_all_controllers', get_all_controllers)
self._request_controller_proxy = rospy.ServiceProxy(
'/controller/request_controller', request_controller)
# Connect push buttons
self._kill_push_btn.toggled.connect(self._on_kill_push_btn_toggle)
self._float_push_btn.toggled.connect(self._on_float_push_btn_toggle)
# Connect combo boxes
self._controller_combo_box.activated[str].connect(
self._controller_combo_box_cb)
# Set up update timer at 10Hz
# A Qt timer is used instead of a ros timer since Qt components are updated
self.update_timer = QTimer()
self.update_timer.timeout.connect(self._onUpdate)
self.update_timer.start(100)
# Temp
self._placeholder_label.setPixmap(self._placeholder_image)
# Everything needs to be turned off here
def shutdown_plugin(self):
self.update_timer.stop()
self._odom_sub.unregister()
del self._odom_sub
self._float_sub.unregister()
del self._float_sub
# Kill broadcaster is not cleared, the user should unkill before closing the GUI
del self._kill_broadcaster
del self._kill_listener
# Subscriber callbacks
# Since this is in a different thread it is possible and likely that
# the drawing thread will try and draw while the text is being changed
# this causes all kinds of mahem such as segmentation faults, double free, ...
# To prevent this from hapening this thread updates only none QT variables
# described here http://wiki.ros.org/rqt/Tutorials/Writing%20a%20Python%20Plugin
def _odom_cb(self, msg):
self.last_odom_msg = msg
def _float_cb(self, status):
self._float_status = status.data
def _current_controller_cb(self, controller):
self._controller = controller.data
self._current_controller = 'Controller: ' + controller.data
def _odom_update(self):
pos = self.last_odom_msg.pose.pose.position
yaw = quat_to_rotvec(
xyzw_array(self.last_odom_msg.pose.pose.orientation))[2]
vel = self.last_odom_msg.twist.twist.linear
dYaw = self.last_odom_msg.twist.twist.angular.z
self._odom_x_label.setText('X: %3.3f' % pos.x)
self._odom_y_label.setText('Y: %3.3f' % pos.y)
self._odom_yaw_label.setText('Yaw: %3.3f' % yaw)
self._odom_d_x_label.setText('dX: %3.3f' % vel.x)
self._odom_d_y_label.setText('dY: %3.3f' % vel.y)
self._odom_d_yaw_label.setText('dYaw: %3.3f' % dYaw)
# Push btn callbacks
def _on_kill_push_btn_toggle(self, checked):
if checked:
self._kill_broadcaster.send(True)
else:
self._kill_broadcaster.send(False)
def _on_float_push_btn_toggle(self, checked):
# TODO: Check if float service active
try:
if checked:
self._float_proxy(True)
else:
self._float_proxy(False)
except rospy.service.ServiceException:
pass
# Combo box callbacks
def _controller_combo_box_cb(self, text):
self._request_controller_proxy(text)
# Update functions
def _updateStatus(self):
# Check if killed
if self._kill_listener.get_killed():
self._kill_label.setPixmap(self._red_indicator)
else:
self._kill_label.setPixmap(self._green_indicator)
# Check float status
if self._float_status:
self._float_label.setPixmap(self._red_indicator)
else:
self._float_label.setPixmap(self._green_indicator)
# Check if in autonomous or RC
self._controller_label.setText(self._current_controller)
def _updateControl(self):
# Wait until we get the first controller
if self._controller == '':
return
controllers = self._get_all_controllers_proxy().controllers
if controllers != self._controllers:
self._controllers = controllers
self._controller_combo_box.clear()
self._controller_combo_box.addItems(controllers)
index = self._controller_combo_box.findText(self._controller)
if index != -1 and index != self._controller_combo_box.currentIndex():
self._controller_combo_box.setCurrentIndex(index)
def _updateLidar(self):
pass
def _onUpdate(self):
self._updateStatus()
self._updateControl()
self._updateLidar()
self._odom_update()
class Controller(object):
def __init__(self, rate=100):
'''I can't swim on account of my ass-mar'''
# Register azi_drive as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
rospy.ServiceProxy('/controller/register_controller', register_controller)('azi_drive')
self.rate = rate
self.servo_max_rotation = 6.0
self.controller_max_rotation = self.servo_max_rotation / self.rate
# rospy.init_node('azi_drive', log_level=rospy.WARN)
# Set up kill handling
self.kill_listener = KillListener(self.on_kill, self.on_unkill)
self.kill_broadcaster = KillBroadcaster(id='Azi_drive', description='Azi drive node shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
self.killed = False
rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(self.controller_max_rotation))
Azi_Drive.set_delta_alpha_max(self.controller_max_rotation)
#Azi_Drive.set_thrust_bound((-50,50))
# These should not be queued! Old commands are garbage.
# Unfortunately, we have to queue these, because the subscriber cannot process two sequential
# thrust messages as quickly as they are sent
self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size=4)
self.servo_pub = rospy.Publisher('controller/dynamixel_full_config', ControlDynamixelFullConfig, queue_size=4)
self.next_wrench = WrenchStamped().wrench
rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size=1)
# Thrust topic id's for each thruster
self.left_id = 3
self.right_id = 2
# Time between messages before azi_drive shuts off
# self.control_timeout = 2 # secs
self.control_timeout = np.inf
self.default_angles = np.array([0.0, 0.0], dtype=np.float32)
self.default_forces = np.array([0.0, 0.0], dtype=np.float32)
self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64)
# Left, Right
self.cur_angles = self.default_angles[:]
self.cur_forces = self.default_forces[:]
self.prev_angles = None
self.set_servo_angles(self.cur_angles)
self.set_forces(self.cur_forces)
# Initializations
self.last_msg_time = time()
self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0
self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors
self.MIN_NEWTONS = -100.0 #Full reverse Jacksons
self.ABS_MAX_PW = .002
self.ABS_MIN_PW = .001
self.ZERO_NEWTONS = 0
self.REV_CONV = (self.ZERO_NEWTONS - self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW)
self.FWD_CONV = (self.MAX_NEWTONS - self.ZERO_NEWTONS) / self.ABS_MAX_PW
class Joystick(object):
def __init__(self):
self.force_scale = rospy.get_param("~force_scale", 600)
self.torque_scale = rospy.get_param("~torque_scale", 500)
self.wrench_choices = itertools.cycle(['rc', 'autonomous'])
self.current_pose = Odometry()
self.active = False
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name='kill', action_required=True, severity=0)
# self.docking_alarm = self.alarm_broadcaster.add_alarm(
# name='docking',
# action_required=True,
# severity=0
# )
self.wrench_pub = rospy.Publisher("/wrench/rc",
WrenchStamped,
queue_size=1)
self.kb = KillBroadcaster(id='station_hold', description='Resets Pose')
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy('/change_wrench',
WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
self.reset()
def reset(self):
'''
Used to reset the state of the controller.
Sometimes when it disconnects then comes back online, the settings are all
out of wack.
'''
self.last_change_mode = False
self.last_station_hold_state = False
self.last_kill = False
self.last_rc = False
self.last_auto = False
self.start_count = 0
self.last_joy = None
self.active = False
self.killed = False
# self.docking = False
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.wrench.force.x = 0
wrench.wrench.force.y = 0
wrench.wrench.torque.z = 0
self.wrench_pub.publish(wrench)
def check_for_timeout(self, joy):
if self.last_joy is None:
self.last_joy = joy
return
if joy.axes == self.last_joy.axes and \
joy.buttons == self.last_joy.buttons:
# No change in state
if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(
15 * 60):
# The controller times out after 15 minutes
if self.active:
rospy.logwarn(
"Controller Timed out. Hold start to resume.")
self.reset()
else:
joy.header.stamp = rospy.Time.now(
) # In the sim, stamps weren't working right
self.last_joy = joy
def joy_cb(self, joy):
self.check_for_timeout(joy)
# Handle Button presses
start = joy.buttons[7]
change_mode = bool(joy.buttons[3]) # Y
kill = bool(joy.buttons[2]) # X
station_hold = bool(joy.buttons[0]) # A
# docking = bool(joy.buttons[1]) # B
rc_control = bool(joy.buttons[11]) # d-pad left
auto_control = bool(joy.buttons[12]) # d-pad right
# Reset controller state if only start is pressed down for awhile
self.start_count += start
if self.start_count > 10: # About 3 seconds
rospy.loginfo("Resetting controller state.")
self.reset()
self.active = True
self.kill_alarm.clear_alarm()
self.wrench_changer("rc")
if not self.active:
return
# Change vehicle mode
if change_mode == 1 and change_mode != self.last_change_mode:
mode = next(self.wrench_choices)
rospy.loginfo("Changing Control Mode: {}".format(mode))
self.wrench_changer(mode)
if rc_control == 1 and rc_control != self.last_rc:
rospy.loginfo("Changing Control to RC")
self.wrench_changer("rc")
if auto_control == 1 and auto_control != self.last_auto:
rospy.loginfo("Changing Control to Autonomous")
self.wrench_changer("autonomous")
# Station hold
if station_hold == 1 and station_hold != self.last_station_hold_state:
rospy.loginfo("Station Holding")
self.kb.send(active=True)
self.kb.send(
active=False) # Resets c3, this'll change when c3 is replaced
self.wrench_changer("autonomous")
# Turn on full system kill
if kill == 1 and kill != self.last_kill:
rospy.loginfo("Toggling Kill")
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(
problem_description='System kill from location: joystick')
self.killed = not self.killed
# # Turn on docking mode
# if docking == 1 and docking != self.last_docking_state:
# rospy.loginfo("Toggling Docking")
# if self.docking:
# self.docking_alarm.clear_alarm()
# else:
# self.docking_alarm.raise_alarm(
# problem_description='Docking kill from location: joystick'
# )
# self.docking = not self.docking
self.last_start = start
self.last_change_mode = change_mode
self.last_kill = kill
self.last_station_hold_state = station_hold
# self.last_docking_state = docking
self.last_auto_control = auto_control
self.last_rc = rc_control
self.last_auto = auto_control
# Handle joystick commands
left_stick_x = joy.axes[1]
left_stick_y = joy.axes[0]
right_stick_y = joy.axes[3]
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.header.stamp = joy.header.stamp
wrench.wrench.force.x = self.force_scale * left_stick_x
wrench.wrench.force.y = self.force_scale * left_stick_y
wrench.wrench.torque.z = self.torque_scale * right_stick_y
self.wrench_pub.publish(wrench)
class Joystick(object):
# Base class for whatever you are writing
def __init__(self):
self.force_scale = rospy.get_param("~force_scale")
self.torque_scale = rospy.get_param("~torque_scale")
self.last_change_mode = False
self.last_station_hold_state = False
self.last_kill = False
self.last_rc = False
self.last_auto = False
self.killed = False
self.docking = False
self.wrench_choices = itertools.cycle(["rc", "autonomous"])
self.current_pose = Odometry()
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(name="kill", action_required=True, severity=0)
self.docking_alarm = self.alarm_broadcaster.add_alarm(name="docking", action_required=True, severity=0)
self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1)
self.kb = KillBroadcaster(id="station_hold", description="Reset Pose")
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
rospy.Subscriber("odom", Odometry, self.odom_cb)
def odom_cb(self, msg):
self.current_pose = msg
def joy_cb(self, joy):
# Handle Button presses
change_mode = bool(joy.buttons[3]) # Y
kill = bool(joy.buttons[2]) # X
station_hold = bool(joy.buttons[0]) # A
docking = bool(joy.buttons[1]) # B
rc_control = bool(joy.buttons[11]) # d-pad left
auto_control = bool(joy.buttons[12]) # d-pad right
# Change vehicle mode
if change_mode == 1 and change_mode != self.last_change_mode:
mode = next(self.wrench_choices)
rospy.loginfo("Changing Control Mode: {}".format(mode))
self.wrench_changer(mode)
if rc_control == 1 and rc_control != self.last_rc:
rospy.loginfo("Changing Control to RC")
self.wrench_changer("rc")
if auto_control == 1 and auto_control != self.last_auto:
rospy.loginfo("Changing Control to Autonomous")
self.wrench_changer("autonomous")
# Station hold
if station_hold == 1 and station_hold != self.last_station_hold_state:
rospy.loginfo("Station Holding")
self.kb.send(active=True)
self.kb.send(active=False) # Resets c3, this'll change when c3 is replaced
self.wrench_changer("autonomous")
# Turn on full system kill
if kill == 1 and kill != self.last_kill:
rospy.loginfo("Toggling Kill")
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(problem_description="System kill from location: joystick")
self.killed = not self.killed
# Turn on docking mode
if docking == 1 and docking != self.last_docking_state:
rospy.loginfo("Toggling Docking")
if self.docking:
self.docking_alarm.clear_alarm()
else:
self.docking_alarm.raise_alarm(problem_description="Docking kill from location: joystick")
self.docking = not self.docking
self.last_change_mode = change_mode
self.last_kill = kill
self.last_station_hold_state = station_hold
self.last_docking_state = docking
self.last_auto_control = auto_control
self.last_rc = rc_control
self.last_auto = auto_control
# Handle joystick commands
left_stick_x = joy.axes[1]
left_stick_y = joy.axes[0]
right_stick_y = joy.axes[3]
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.wrench.force.x = self.force_scale * left_stick_x
wrench.wrench.force.y = self.force_scale * left_stick_y
wrench.wrench.torque.z = self.torque_scale * right_stick_y
self.wrench_pub.publish(wrench)
def __init__(self):
# Initilize ros
rospy.init_node('control_arbiter')
# Vars
self.controller = 'none'
self.controllers = []
self.floating = False
self.killed = False
self.continuous_angle_lock = threading.Lock()
self.joint_lock = threading.Lock()
self.full_lock = threading.Lock()
self.wheel_lock = threading.Lock()
self.thrust_lock = threading.Lock()
# Services
self.request_controller_srv = rospy.Service('request_controller',
request_controller,
self.request_controller_cb)
self.register_controller_srv = rospy.Service(
'register_controller', register_controller,
self.register_controller_cb)
self.float_srv = rospy.Service('float_mode', FloatMode,
self.set_float_mode)
self.get_all_controllers_srv = rospy.Service(
'get_all_controllers', get_all_controllers,
self.get_all_controllers_cb)
# Publishers
self.continuous_angle_pub = rospy.Publisher(
'/dynamixel/dynamixel_continuous_angle_config',
DynamixelContinuousAngleConfig,
queue_size=10)
self.full_pub = rospy.Publisher('/dynamixel/dynamixel_full_config',
DynamixelFullConfig,
queue_size=10)
self.joint_pub = rospy.Publisher('/dynamixel/dynamixel_joint_config',
DynamixelJointConfig,
queue_size=10)
self.wheel_pub = rospy.Publisher('/dynamixel/dynamixel_wheel_config',
DynamixelWheelConfig,
queue_size=10)
self.port_pub = rospy.Publisher('/stm32f3discovery_imu_driver/pwm1',
Float64,
queue_size=10)
self.starboard_pub = rospy.Publisher(
'/stm32f3discovery_imu_driver/pwm2', Float64, queue_size=10)
self.float_status_pub = rospy.Publisher('float_status',
Bool,
queue_size=1)
self.current_controller_pub = rospy.Publisher('current_controller',
String,
queue_size=1)
# Subscribers
self.continuous_angle_sub = rospy.Subscriber(
'dynamixel_continuous_angle_config',
ControlDynamixelContinuousAngleConfig,
self.continuous_angle_cb,
queue_size=10)
self.full_sub = rospy.Subscriber('dynamixel_full_config',
ControlDynamixelFullConfig,
self.full_cb,
queue_size=10)
self.joint_sub = rospy.Subscriber('dynamixel_joint_config',
ControlDynamixelJointConfig,
self.joint_cb,
queue_size=10)
self.wheel_sub = rospy.Subscriber('dynamixel_wheel_config',
ControlDynamixelWheelConfig,
self.wheel_cb,
queue_size=10)
self.thruster_sub = rospy.Subscriber('thruster_config',
ControlThrustConfig,
self.thruster_cb,
queue_size=10)
# Timers
self.update_timer = rospy.Timer(rospy.Duration(0.1),
self.status_update)
# Kill
self.kill_listener = KillListener(self.set_kill_cb, self.clear_kill_cb)
self.kill_broadcaster = KillBroadcaster(
id='control_arbiter', description='control_arbiter has shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
#!/usr/bin/env python
import rospy
import time
import serial
from kill_handling.listener import KillListener
from kill_handling.broadcaster import KillBroadcaster
from std_msgs.msg import Bool
try:
rospy.init_node('kill_switch')
kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='kill button kill')
begin = rospy.Publisher("begin", Bool, queue_size = 1)
last_pressed = ''
killed = True
# configure the serial connections (the parameters differs on the device you are connecting to)
ser = serial.Serial(
port='/dev/serial/by-id/usb-Black_Sphere_Technologies_CDC-ACM_Demo_DEMO-if00',
baudrate=9600,
#parity=serial.PARITY_ODD,
#stopbits=serial.STOPBITS_TWO,
#bytesize=serial.SEVENBITS
)
ser.close()
ser.open()
ser.isOpen()
kill_broadcaster.send(killed)
ser.write('B')
print 'Enter your commands below.\r\nInsert "exit" to leave the application.'
input=1
while not rospy.is_shutdown() :
