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))
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, 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 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):
# 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):
# 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))
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 __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)
def __init__(self): # Initilization code self.killed = False self.kill_listener = KillListener(self.set_kill, self.clear_kill)