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 __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))
class send_action:
def __init__(self, name):
self.action = actionlib.SimpleActionServer('moveto', MoveToAction, self.new_goal, False)
self.trajectory_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size=10)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.action.start()
self.to_pose = PoseTwistStamped()
self.temp_pose = PoseTwistStamped()
self.killed = False
self.waypoint = False
def callback(self, msg):
self.waypoint = msg.data
def set_kill(self):
self.to_pose = PoseTwistStamped()
# TODO --> Verify that all values are at 0
#members = [attr for attr in dir(self.to_pose.posetwist.pose) if attr.startswith("position") or attr.startswith("orientation")]
#getattr(self.to_pose.posetwist.pose.position, "x") == 0
rospy.logwarn('Azi_Drive waypoint kill flag off -- All trajectories at 0: %s' % self.kill_listener.get_kills())
self.killed = True
def clear_kill(self):
self.killed = False
rospy.logwarn('Azi_Drive waypoint kill flag off -- Waypoints enabled: %s' % self.kill_listener.get_kills())
def new_goal(self, goal):
self.waypoint = False
self.temp_pose = PoseTwistStamped()
self.temp_pose.posetwist = goal.posetwist
self.temp_pose.header = goal.header
time.sleep(5)
self.waypoint_progress = rospy.Subscriber('/waypoint_progress', Bool, self.callback)
while self.waypoint == False:
None
self.action.set_succeeded()
self.waypoint = True
def over_and_over(self):
r = rospy.Rate(1)
if self.killed == True:
rospy.logwarn('Azi_Drive waypoint kill flag on -- Waypoints disabled: %s' % self.kill_listener.get_kills())
if self.killed == False:
self.to_pose = self.temp_pose
self.trajectory_pub.publish(self.to_pose)
print self.to_pose.posetwist.pose
self.to_pose = PoseTwistStamped()
r.sleep()
def __init__(self, name):
self.action = actionlib.SimpleActionServer('moveto', MoveToAction, self.new_goal, False)
self.trajectory_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size=10)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.action.start()
self.to_pose = PoseTwistStamped()
self.temp_pose = PoseTwistStamped()
self.killed = False
self.waypoint = False
def __init__(self, plan_names, master_timeout=None):
self._plans = PlanSet(plan_names)
self._master_timeout = master_timeout
self._sm = None
self._shared = uf_smach.util.StateSharedHandles()
self._pub = rospy.Publisher('mission/plans', PlansStamped)
self._srv = rospy.Service('mission/modify_plan', ModifyPlan, self._modify_plan)
self._run_srv = actionlib.SimpleActionServer('mission/run', RunMissionsAction,
self.execute, False)
self._run_srv.register_preempt_callback(self._on_preempt)
self._run_srv.start()
self._tim = rospy.Timer(rospy.Duration(.1), lambda _: self._publish_plans())
self._kill_listener = KillListener(self._on_preempt)
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))
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 __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 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))
class MissionServer(object):
def __init__(self, plan_names, master_timeout=None):
self._plans = PlanSet(plan_names)
self._master_timeout = master_timeout
self._sm = None
self._shared = uf_smach.util.StateSharedHandles()
self._pub = rospy.Publisher('mission/plans', PlansStamped)
self._srv = rospy.Service('mission/modify_plan', ModifyPlan, self._modify_plan)
self._run_srv = actionlib.SimpleActionServer('mission/run', RunMissionsAction,
self.execute, False)
self._run_srv.register_preempt_callback(self._on_preempt)
self._run_srv.start()
self._tim = rospy.Timer(rospy.Duration(.1), lambda _: self._publish_plans())
self._kill_listener = KillListener(self._on_preempt)
def get_plan(self, plan):
return self._plans.get_plan(plan)
def execute(self, goal):
self._sm = self._plans.make_sm(self._shared, self._master_timeout)
sis = smach_ros.IntrospectionServer('mission_planner', self._sm, '/SM_ROOT')
sis.start()
print 'Waiting for unkill'
while self._kill_listener.get_killed():
rospy.sleep(.1)
outcome = self._sm.execute()
sis.stop()
self._shared['moveto'].cancel_goal()
if outcome == 'succeeded':
self._run_srv.set_succeeded(RunMissionsResult(outcome))
else:
self._run_srv.set_preempted()
def _on_preempt(self):
if self._sm is not None:
self._sm.request_preempt()
def _publish_plans(self):
self._pub.publish(PlansStamped(
header=Header(stamp=rospy.Time.now()),
plans=[Plan(name=name,
entries=[uf_smach.msg.PlanEntry(entry.mission, rospy.Duration(entry.timeout),
entry.contigency_plan, entry.path, entry.dist)
for entry in self._plans.get_plan(name)])
for name in self._plans.get_plans()],
available_missions=get_missions()))
def _modify_plan(self, req):
if req.plan not in self._plans.get_plans():
return None
plan = self._plans.get_plan(req.plan)
entry = PlanEntry(req.entry.mission,
req.entry.timeout.to_sec(),
req.entry.contigency_plan if len(req.entry.contigency_plan) > 0 else None,
req.entry.path if req.entry.path != 'none' else None,
req.entry.dist)
if req.operation == ModifyPlanRequest.INSERT:
if req.pos > len(plan):
return None
plan.insert(req.pos, entry)
elif req.operation == ModifyPlanRequest.REMOVE:
if req.pos >= len(plan):
return None
del plan[req.pos]
elif req.operation == ModifyPlanRequest.REPLACE:
if req.pos >= len(plan):
return None
plan[req.pos] = entry
else:
return None
return ()
class Controller(object):
def __init__(self):
# Old gains
self.d_x = 175
self.d_y = 90
self.d_z = 60
self.p_x = 30
self.p_y = 30
self.p_z = 40
#self.d_x = 175
#self.d_y = 90
#self.d_z = 90
#self.p_x = 40
#self.p_y = 40
#self.p_z = 100
self.killed = False
self.enable = True
self.odom_active = False
self.K_d = numpy.array([
[self.d_x,0,0,0,0,0],
[0,self.d_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.d_z]])
self.K_p = numpy.array([
[self.p_x,0,0,0,0,0],
[0,self.p_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.p_z]])
# Averaging parameters
self.last_average = numpy.zeros(6)
self.num_to_avg = 75 # At 50 hz this is 50 samples is one second of data
self.outputs = deque([numpy.zeros(6) for i in range(0, self.num_to_avg)])
self.desired_state_set = False
self.desired_state = numpy.ones(6)
self.desired_state_dot = numpy.ones(6)
self.state = numpy.ones(6)
self.previous_error = numpy.ones(6)
self.state_dot = numpy.ones(6)
self.state_dot_body = numpy.ones(6)
self.lock = threading.Lock()
# Get tf listener
self.tf_listener = tf.TransformListener()
rospy.Subscriber("pd_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pd_p_gain", Point, self.p_gain_callback)
rospy.Subscriber('/trajectory', PoseTwistStamped, self.desired_state_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.controller_wrench = rospy.Publisher('wrench', WrenchStamped, queue_size = 1)
self.waypoint_progress = rospy.Publisher('waypoint_progress', Bool, queue_size = 1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.z_error = 0
self.x_error = 0
self.y_error = 0
self.dz_error = 0
self.dx_error = 0
self.dy_error = 0
def set_kill(self):
self.killed = True
rospy.logwarn('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
def clear_kill(self):
self.killed = False
rospy.logwarn('PD_Controller ACTIVE: %s' % self.kill_listener.get_kills())
def d_gain_callback(self, msg):
self.d_x = msg.x
self.d_y = msg.y
self.d_z = msg.z
self.K_d = numpy.array([
[self.d_x,0,0,0,0,0],
[0,self.d_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.d_z]])
def p_gain_callback(self, msg):
self.p_x = msg.x
self.p_y = msg.y
self.p_z = msg.z
self.K_p = numpy.array([
[self.p_x,0,0,0,0,0],
[0,self.p_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.p_z]])
def desired_state_callback(self,desired_posetwist):
self.lock.acquire()
self.desired_state_set = True
self.desired_state = numpy.concatenate([xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion(xyzw_array(desired_posetwist.posetwist.pose.orientation))])
self.desired_state_dot = numpy.concatenate([xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular)])
self.lock.release()
def odom_callback(self, current_posetwist):
self.lock.acquire()
self.odom_active = True
# Grab position; 0 Z
pose = xyz_array(current_posetwist.pose.pose.position)
pose[2] = 0
# Grab orientation; 0 pitch and roll
orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation)))
orientation[0:2] = 0
self.state = numpy.concatenate([xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation))])
self.state_dot = numpy.concatenate([xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)])
if (not self.desired_state_set):
self.desired_state = self.state
self.desired_state_set = True
self.lock.release()
def main_loop(self, event):
self.lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2*math.pi) - math.pi
# sub pd-controller sans rise
rot = None
# Get tf from /enu to /base_link
# Since we are dealing with differences and not absolute positions not translation is required
try:
(_, rot) = self.tf_listener.lookupTransform('/base_link', '/enu', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn('Tf exception: ' + str(e))
if rot is not None:
# Convert to euler angle ( we only care about rotation about z)
theta = quat_to_rotvec(numpy.array(rot))[2]
# Difference in /enu frame
to_desired_state = self.desired_state[0:2] - self.state[0:2]
# Convert to base_link
s = numpy.sin(theta)
c = numpy.cos(theta)
rot_matrix_enu_base = numpy.array([[c, -s],
[s, c]])
to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
to_desired_state = numpy.insert(to_desired_state, 2, 0) # Append a zero for z
# Note: Angular differences are the same in base_link as enu so no tf required
e = numpy.concatenate([to_desired_state, map(smallest_coterminal_angle, self.desired_state[3:6] - self.state[3:6])]) # e_1 in paper
# Convert desired state dot into base_link
# angular velocities do not rquire transformation as they are differences (rendering them frameless)
# To convert velocites from enu + desired_orientation to base link:
# transform to enu: rotate the velocity vector by desired_orientation
# transform to base link: rotate the velocity vector by the angle between enu and base_link
vels = self.desired_state_dot[0:2]
theta2 = self.desired_state[5]
s = numpy.sin(theta2)
c = numpy.cos(theta2)
rot_matrix_desired_enu = numpy.array([[c, -s],
[s, c]])
vels = rot_matrix_desired_enu.dot(vels)
vels = rot_matrix_enu_base.dot(vels)
vels = numpy.insert(vels, 2, 0)
desired_state_dot = numpy.concatenate([vels, self.desired_state_dot[3:6]])
#print 'Desired_state tf: ', desired_state_dot
e_dot = desired_state_dot - self.state_dot
output = self.K_p.dot(e) + self.K_d.dot(e_dot)
# Apply simple moving average filter
new = output / self.num_to_avg
old = (self.outputs[0] / self.num_to_avg)
self.last_average = self.last_average - old + new
self.outputs.popleft()
self.outputs.append(output)
# Resuse output var
#print 'Outputs: ', self.outputs
output = self.last_average
#print 'Last Average: ', output
self.lock.release()
self.x_error = e[0]
self.y_error = e[1]
self.z_error = e[5]
self.dx_error = e_dot[0]
self.dy_error = e_dot[1]
self.dz_error = e_dot[5]
#self.to_terminal()
if (not(self.odom_active)):
output = [0,0,0,0,0,0]
if (self.enable & self.killed==False):
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= output[0],y= output[1],z= 0),
torque = Vector3(x=0,y= 0,z= output[5]),
))
)
if((self.x_error < 1) & (self.y_error < 1) & (self.z_error < 1)):
self.waypoint_progress.publish(True)
if (self.killed == True):
rospy.logwarn('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= 0,y= 0,z= 0),
torque = Vector3(x=0,y= 0,z= 0),
))
)
else:
self.lock.release()
def timeout_callback(self, event):
self.odom_active = False
def to_terminal(self):
print "X: ", self.x_error
print "Y: ", self.y_error
print "Z: ", self.z_error
print "dX: ", self.dx_error
print "dY: ", self.dy_error
print "dZ: ", self.dz_error
def __init__(self):
# Old gains
self.d_x = 175
self.d_y = 90
self.d_z = 60
self.p_x = 30
self.p_y = 30
self.p_z = 40
#self.d_x = 175
#self.d_y = 90
#self.d_z = 90
#self.p_x = 40
#self.p_y = 40
#self.p_z = 100
self.killed = False
self.enable = True
self.odom_active = False
self.K_d = numpy.array([
[self.d_x,0,0,0,0,0],
[0,self.d_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.d_z]])
self.K_p = numpy.array([
[self.p_x,0,0,0,0,0],
[0,self.p_y,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,self.p_z]])
# Averaging parameters
self.last_average = numpy.zeros(6)
self.num_to_avg = 75 # At 50 hz this is 50 samples is one second of data
self.outputs = deque([numpy.zeros(6) for i in range(0, self.num_to_avg)])
self.desired_state_set = False
self.desired_state = numpy.ones(6)
self.desired_state_dot = numpy.ones(6)
self.state = numpy.ones(6)
self.previous_error = numpy.ones(6)
self.state_dot = numpy.ones(6)
self.state_dot_body = numpy.ones(6)
self.lock = threading.Lock()
# Get tf listener
self.tf_listener = tf.TransformListener()
rospy.Subscriber("pd_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pd_p_gain", Point, self.p_gain_callback)
rospy.Subscriber('/trajectory', PoseTwistStamped, self.desired_state_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.controller_wrench = rospy.Publisher('wrench', WrenchStamped, queue_size = 1)
self.waypoint_progress = rospy.Publisher('waypoint_progress', Bool, queue_size = 1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.z_error = 0
self.x_error = 0
self.y_error = 0
self.dz_error = 0
self.dx_error = 0
self.dy_error = 0
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):
# Old gains
self.d_x = 175
self.d_y = 90
self.d_z = 60
self.p_x = 30
self.p_y = 30
self.p_z = 40
#self.d_x = 175
#self.d_y = 90
#self.d_z = 90
#self.p_x = 40
#self.p_y = 40
#self.p_z = 100
self.killed = False
self.enable = True
self.odom_active = False
self.K_d = numpy.array([[self.d_x, 0, 0, 0, 0, 0],
[0, self.d_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.d_z]])
self.K_p = numpy.array([[self.p_x, 0, 0, 0, 0, 0],
[0, self.p_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.p_z]])
# Averaging parameters
self.last_average = numpy.zeros(6)
self.num_to_avg = 75 # At 50 hz this is 50 samples is one second of data
self.outputs = deque(
[numpy.zeros(6) for i in range(0, self.num_to_avg)])
self.desired_state_set = False
self.desired_state = numpy.ones(6)
self.desired_state_dot = numpy.ones(6)
self.state = numpy.ones(6)
self.previous_error = numpy.ones(6)
self.state_dot = numpy.ones(6)
self.state_dot_body = numpy.ones(6)
self.lock = threading.Lock()
# Get tf listener
self.tf_listener = tf.TransformListener()
rospy.Subscriber("pd_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pd_p_gain", Point, self.p_gain_callback)
rospy.Subscriber('/trajectory', PoseTwistStamped,
self.desired_state_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.controller_wrench = rospy.Publisher('wrench',
WrenchStamped,
queue_size=1)
self.waypoint_progress = rospy.Publisher('waypoint_progress',
Bool,
queue_size=1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.z_error = 0
self.x_error = 0
self.y_error = 0
self.dz_error = 0
self.dx_error = 0
self.dy_error = 0
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))
class Controller(object):
def __init__(self):
# Old gains
self.d_x = 175
self.d_y = 90
self.d_z = 60
self.p_x = 30
self.p_y = 30
self.p_z = 40
#self.d_x = 175
#self.d_y = 90
#self.d_z = 90
#self.p_x = 40
#self.p_y = 40
#self.p_z = 100
self.killed = False
self.enable = True
self.odom_active = False
self.K_d = numpy.array([[self.d_x, 0, 0, 0, 0, 0],
[0, self.d_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.d_z]])
self.K_p = numpy.array([[self.p_x, 0, 0, 0, 0, 0],
[0, self.p_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.p_z]])
# Averaging parameters
self.last_average = numpy.zeros(6)
self.num_to_avg = 75 # At 50 hz this is 50 samples is one second of data
self.outputs = deque(
[numpy.zeros(6) for i in range(0, self.num_to_avg)])
self.desired_state_set = False
self.desired_state = numpy.ones(6)
self.desired_state_dot = numpy.ones(6)
self.state = numpy.ones(6)
self.previous_error = numpy.ones(6)
self.state_dot = numpy.ones(6)
self.state_dot_body = numpy.ones(6)
self.lock = threading.Lock()
# Get tf listener
self.tf_listener = tf.TransformListener()
rospy.Subscriber("pd_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pd_p_gain", Point, self.p_gain_callback)
rospy.Subscriber('/trajectory', PoseTwistStamped,
self.desired_state_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.controller_wrench = rospy.Publisher('wrench',
WrenchStamped,
queue_size=1)
self.waypoint_progress = rospy.Publisher('waypoint_progress',
Bool,
queue_size=1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.z_error = 0
self.x_error = 0
self.y_error = 0
self.dz_error = 0
self.dx_error = 0
self.dy_error = 0
def set_kill(self):
self.killed = True
rospy.logwarn('PD_Controller KILLED: %s' %
self.kill_listener.get_kills())
def clear_kill(self):
self.killed = False
rospy.logwarn('PD_Controller ACTIVE: %s' %
self.kill_listener.get_kills())
def d_gain_callback(self, msg):
self.d_x = msg.x
self.d_y = msg.y
self.d_z = msg.z
self.K_d = numpy.array([[self.d_x, 0, 0, 0, 0, 0],
[0, self.d_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.d_z]])
def p_gain_callback(self, msg):
self.p_x = msg.x
self.p_y = msg.y
self.p_z = msg.z
self.K_p = numpy.array([[self.p_x, 0, 0, 0, 0, 0],
[0, self.p_y, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, self.p_z]])
def desired_state_callback(self, desired_posetwist):
self.lock.acquire()
self.desired_state_set = True
self.desired_state = numpy.concatenate([
xyz_array(desired_posetwist.posetwist.pose.position),
transformations.euler_from_quaternion(
xyzw_array(desired_posetwist.posetwist.pose.orientation))
])
self.desired_state_dot = numpy.concatenate([
xyz_array(desired_posetwist.posetwist.twist.linear),
xyz_array(desired_posetwist.posetwist.twist.angular)
])
self.lock.release()
def odom_callback(self, current_posetwist):
self.lock.acquire()
self.odom_active = True
# Grab position; 0 Z
pose = xyz_array(current_posetwist.pose.pose.position)
pose[2] = 0
# Grab orientation; 0 pitch and roll
orientation = numpy.array(
transformations.euler_from_quaternion(
xyzw_array(current_posetwist.pose.pose.orientation)))
orientation[0:2] = 0
self.state = numpy.concatenate([
xyz_array(current_posetwist.pose.pose.position),
transformations.euler_from_quaternion(
xyzw_array(current_posetwist.pose.pose.orientation))
])
self.state_dot = numpy.concatenate([
xyz_array(current_posetwist.twist.twist.linear),
xyz_array(current_posetwist.twist.twist.angular)
])
if (not self.desired_state_set):
self.desired_state = self.state
self.desired_state_set = True
self.lock.release()
def main_loop(self, event):
self.lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2 * math.pi) - math.pi
# sub pd-controller sans rise
rot = None
# Get tf from /enu to /base_link
# Since we are dealing with differences and not absolute positions not translation is required
try:
(_,
rot) = self.tf_listener.lookupTransform('/base_link', '/enu',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn('Tf exception: ' + str(e))
if rot is not None:
# Convert to euler angle ( we only care about rotation about z)
theta = quat_to_rotvec(numpy.array(rot))[2]
# Difference in /enu frame
to_desired_state = self.desired_state[0:2] - self.state[0:2]
# Convert to base_link
s = numpy.sin(theta)
c = numpy.cos(theta)
rot_matrix_enu_base = numpy.array([[c, -s], [s, c]])
to_desired_state = rot_matrix_enu_base.dot(to_desired_state)
to_desired_state = numpy.insert(to_desired_state, 2,
0) # Append a zero for z
# Note: Angular differences are the same in base_link as enu so no tf required
e = numpy.concatenate([
to_desired_state,
map(smallest_coterminal_angle,
self.desired_state[3:6] - self.state[3:6])
]) # e_1 in paper
# Convert desired state dot into base_link
# angular velocities do not rquire transformation as they are differences (rendering them frameless)
# To convert velocites from enu + desired_orientation to base link:
# transform to enu: rotate the velocity vector by desired_orientation
# transform to base link: rotate the velocity vector by the angle between enu and base_link
vels = self.desired_state_dot[0:2]
theta2 = self.desired_state[5]
s = numpy.sin(theta2)
c = numpy.cos(theta2)
rot_matrix_desired_enu = numpy.array([[c, -s], [s, c]])
vels = rot_matrix_desired_enu.dot(vels)
vels = rot_matrix_enu_base.dot(vels)
vels = numpy.insert(vels, 2, 0)
desired_state_dot = numpy.concatenate(
[vels, self.desired_state_dot[3:6]])
#print 'Desired_state tf: ', desired_state_dot
e_dot = desired_state_dot - self.state_dot
output = self.K_p.dot(e) + self.K_d.dot(e_dot)
# Apply simple moving average filter
new = output / self.num_to_avg
old = (self.outputs[0] / self.num_to_avg)
self.last_average = self.last_average - old + new
self.outputs.popleft()
self.outputs.append(output)
# Resuse output var
#print 'Outputs: ', self.outputs
output = self.last_average
#print 'Last Average: ', output
self.lock.release()
self.x_error = e[0]
self.y_error = e[1]
self.z_error = e[5]
self.dx_error = e_dot[0]
self.dy_error = e_dot[1]
self.dz_error = e_dot[5]
#self.to_terminal()
if (not (self.odom_active)):
output = [0, 0, 0, 0, 0, 0]
if (self.enable & self.killed == False):
self.controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=output[0],
y=output[1],
z=0),
torque=Vector3(x=0, y=0, z=output[5]),
)))
if ((self.x_error < 1) & (self.y_error < 1) &
(self.z_error < 1)):
self.waypoint_progress.publish(True)
if (self.killed == True):
rospy.logwarn('PD_Controller KILLED: %s' %
self.kill_listener.get_kills())
self.controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=0, y=0, z=0),
torque=Vector3(x=0, y=0, z=0),
)))
else:
self.lock.release()
def timeout_callback(self, event):
self.odom_active = False
def to_terminal(self):
print "X: ", self.x_error
print "Y: ", self.y_error
print "Z: ", self.z_error
print "dX: ", self.dx_error
print "dY: ", self.dy_error
print "dZ: ", self.dz_error
def __init__(self):
'''
Structure gain array for flexible use
This allows the gain function to access all gains by simple indexing scheme
Place x and y gains in the first row of the 6x3
Place x gains in the bottom row
the i_history array uses this scheme as well
Gain array layout:
[ p_x. i_x. d_x.]
[ p_y. i_y. d_y.]
[ 0. 0. 0.]
[ 0. 0. 0.]
[ 0. 0. 0.]
[ p_z. i_z. d_z.]
'''
self.K = numpy.zeros((6,3))
self.K[0:6, 0:6] = [
[p_x, i_x, d_x], # pid_x
[p_y, i_y, d_y], # pid_y
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
[p_z, i_z, d_z], # pid_z
]
# Kill functions
self.odom_active = False
self.killed = False
# Create arrays to be used
self.desired_state = numpy.ones(6)
self.desired_velocity = numpy.ones(6)
self.current_state = numpy.zeros(6)
self.current_velocity = numpy.zeros(6)
self.current_error = numpy.ones(6)
self.i_history = [[0 for x in range(1)] for x in range(6)]
self.i_history[0] = deque()
self.i_history[1] = deque()
self.i_history[5] = deque()
self.integrator = numpy.zeros(6)
self.Derivator= 0
self.lock = threading.Lock()
# Used to reset the desired state on startup
self.desired_state_set = False
# ROS components
self.controller_wrench = rospy.Publisher('wrench', WrenchStamped, queue_size = 1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
rospy.Subscriber('/trajectory', PoseTwistStamped, self.trajectory_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
rospy.Subscriber("pid_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pid_p_gain", Point, self.p_gain_callback)
rospy.Subscriber("pid_i_gain", Point, self.i_gain_callback)
class PID_controller:
def __init__(self):
'''
Structure gain array for flexible use
This allows the gain function to access all gains by simple indexing scheme
Place x and y gains in the first row of the 6x3
Place x gains in the bottom row
the i_history array uses this scheme as well
Gain array layout:
[ p_x. i_x. d_x.]
[ p_y. i_y. d_y.]
[ 0. 0. 0.]
[ 0. 0. 0.]
[ 0. 0. 0.]
[ p_z. i_z. d_z.]
'''
self.K = numpy.zeros((6,3))
self.K[0:6, 0:6] = [
[p_x, i_x, d_x], # pid_x
[p_y, i_y, d_y], # pid_y
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
[p_z, i_z, d_z], # pid_z
]
# Kill functions
self.odom_active = False
self.killed = False
# Create arrays to be used
self.desired_state = numpy.ones(6)
self.desired_velocity = numpy.ones(6)
self.current_state = numpy.zeros(6)
self.current_velocity = numpy.zeros(6)
self.current_error = numpy.ones(6)
self.i_history = [[0 for x in range(1)] for x in range(6)]
self.i_history[0] = deque()
self.i_history[1] = deque()
self.i_history[5] = deque()
self.integrator = numpy.zeros(6)
self.Derivator= 0
self.lock = threading.Lock()
# Used to reset the desired state on startup
self.desired_state_set = False
# ROS components
self.controller_wrench = rospy.Publisher('wrench', WrenchStamped, queue_size = 1)
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
rospy.Subscriber('/trajectory', PoseTwistStamped, self.trajectory_callback)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
rospy.Subscriber("pid_d_gain", Point, self.d_gain_callback)
rospy.Subscriber("pid_p_gain", Point, self.p_gain_callback)
rospy.Subscriber("pid_i_gain", Point, self.i_gain_callback)
def p_gain_callback(self, msg):
x = msg.x
y = msg.y
z = msg.z
self.K[0:2, 0] = [x,y]
self.K[5, 0] = z
def i_gain_callback(self, msg):
x = msg.x
y = msg.y
z = msg.z
self.K[0:2, 1] = [x,y]
self.K[5, 1] = z
def d_gain_callback(self, msg):
x = msg.x
y = msg.y
z = msg.z
self.K[0:2, 2] = [x,y]
self.K[5, 2] = z
def set_kill(self):
self.killed = True
rospy.logdebug('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
def clear_kill(self):
self.killed = False
rospy.logdebug('PD_Controller ACTIVE: %s' % self.kill_listener.get_kills())
def trajectory_callback(self, desired_trajectory):
self.lock.acquire()
self.desired_state_set = True
# Get desired pose and orientation
desired_pose = xyz_array(desired_trajectory.posetwist.pose.position)
desired_orientation = transformations.euler_from_quaternion(xyzw_array(desired_trajectory.posetwist.pose.orientation))
# Get desired linear and angular velocities
desired_lin_vel = xyz_array(desired_trajectory.posetwist.twist.linear)
desired_ang_vel = xyz_array(desired_trajectory.posetwist.twist.angular)
# Add desired position to desired state i_historys
self.desired_state = numpy.concatenate([desired_pose, desired_orientation])
# Add desired velocities to velocity i_history
self.desired_velocity = numpy.concatenate([desired_lin_vel, desired_ang_vel])
self.lock.release()
def odom_callback(self, current_pos):
self.lock.acquire()
self.odom_active = True
# Grab current position and orientation and 0 linear Z and angluar X and Y
# Get current position
current_position = xyz_array(current_pos.pose.pose.position)
current_orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_pos.pose.pose.orientation)))
# Zero unneccesary elements
current_position[2] = 0
current_orientation[0:2] = 0
# Add current position to state i_history
self.current_state = numpy.concatenate([current_position, current_orientation])
# Get current velocities
current_lin_vel = xyz_array(current_pos.twist.twist.linear)
current_ang_vel = xyz_array(current_pos.twist.twist.angular)
# Add current velocities to velocity i_history
self.current_velocity = numpy.concatenate([current_lin_vel, current_ang_vel])
# If the desired state has not yet been set, set desired and current as the same
# Resets the controller to current position on bootup
if (not self.desired_state_set):
self.desired_state = self.current_state
self.desired_state_set = True
self.lock.release()
def PID(self, variable):
# Index in state number we want to access
state_number = 0
if variable == 'x': state_number = 0
if variable == 'y': state_number = 1
if variable == 'z': state_number = 5
#self.current_error = self.desired_state[state_number] - self.current_state[state_number]
#rospy.logdebug(variable + ": " + str(self.current_error[state_number]))
p = self.K[state_number, 0] * self.current_error[state_number]
i = (self.integrator[state_number] + self.current_error[state_number]) * self.K[state_number, 1]
d = self.K[state_number, 2] * (self.current_error[state_number] - self.Derivator)
# This section will be the FOPID implimentation, but I am still working on it
if abs(self.current_error[state_number]) > 0: pass
#i = i * (1 + abs(d))
rospy.logdebug(self.current_error[state_number])
rospy.logwarn('P' + variable + ": " + str(p))
rospy.logwarn('I' + variable + ": " + str(i))
rospy.logwarn('D' + variable + ": " + str(d))
# Set temporary variable for use in integrator sliding window
sliding_window = self.i_history[state_number]
# append to integrator array
sliding_window.append(i)
# If array is larger than 5 items, remove item
if len(sliding_window) > 5:
sliding_window.pop()
# Set up variables for next iteration
# Sum only last 5 numbers of intergration
self.Derivator = self.current_error[state_number]
self.integrator[state_number] = sum(sliding_window)
PID = p + i + d
return PID
def timeout_callback(self, event):
self.odom_active = False
def jacobian(self, x):
# maps global linear velocity/euler rates -> body linear+angular velocities
sphi, cphi = math.sin(x[3]), math.cos(x[3])
stheta, ctheta = math.sin(x[4]), math.cos(x[4])
spsi, cpsi = math.sin(x[5]), math.cos(x[5])
J_inv = numpy.zeros((6, 6))
J_inv[0:3, 0:3] = [
[ ctheta * cpsi , ctheta * spsi , -stheta],
[sphi * stheta * cpsi - cphi * spsi, sphi * stheta * spsi + cphi * cpsi, sphi * ctheta],
[cphi * stheta * cpsi + sphi * spsi, cphi * stheta * spsi - sphi * cpsi, cphi * ctheta],]
J_inv[3:6, 3:6] = [
[1, 0, -stheta],
[0, cphi, sphi * ctheta],
[0, -sphi, cphi * ctheta],]
return J_inv
def main_loop(self, event):
def smallest_coterminal_angle(x):
return (x + math.pi) % (2*math.pi) - math.pi
self.lock.acquire()
# Get linear and angular error between desired and current pose - /enu
linear_error = self.desired_state[0:3] - self.current_state[0:3]
angular_error = map(smallest_coterminal_angle, self.desired_state[3:6] - self.current_state[3:6])
# Combine errors into one array
error_enu = numpy.concatenate([linear_error, angular_error])
#rospy.logdebug(error_enu)
# Translate /enu errors into /base_link errors
error_base = self.jacobian(self.current_state).dot(error_enu)
# Take away velocity from error to avoid overshoot
final_error = error_base - self.current_velocity
# Place errors to be sent into main error array
self.current_error = [final_error[0], final_error[1], 0, 0, 0, final_error[5]]
self.lock.release()
# Send error values through PID controller
x = self.PID('x')
y = self.PID('y')
z = self.PID('z')
# Combine into final wrenches
wrench = [x,y,0,0,0,z]
# If odometry has not been aquired, set wrench to 0
if (not(self.odom_active)):
wrench = [0,0,0,0,0,0]
# If ready to go...
if (self.killed == False):
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= wrench[0],y= wrench[1],z= 0),
torque = Vector3(x=0,y= 0,z= wrench[5]),
))
)
# If not ready to go...
if (self.killed == True):
rospy.logdebug('PD_Controller KILLED: %s' % self.kill_listener.get_kills())
self.controller_wrench.publish(WrenchStamped(
header = Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force = Vector3(x= 0,y= 0,z= 0),
torque = Vector3(x=0,y= 0,z= 0),
))
)
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()
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 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))
def __init__(self): # Initilization code self.killed = False self.kill_listener = KillListener(self.set_kill, self.clear_kill)
