void JointVelocityController::update(const ros::Time& time, const ros::Duration& period)
{
double error = command_ - joint_.getVelocity();
// Set the PID error and compute the PID command with nonuniform time
// step size. The derivative error is computed from the change in the error
// and the timestep dt.
double commanded_effort = pid_controller_.computeCommand(error, period);
joint_.setCommand(commanded_effort);
if(loop_count_ % 10 == 0)
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command_;
controller_state_publisher_->msg_.process_value = joint_.getVelocity();
controller_state_publisher_->msg_.error = error;
controller_state_publisher_->msg_.time_step = period.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
dummy);
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
}
void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
{
double command = *(command_.readFromRT());
double error, vel_error;
// Compute position error
if (joint_urdf_->type == urdf::Joint::REVOLUTE)
{
angles::shortest_angular_distance_with_limits(joint_.getPosition(),
command,
joint_urdf_->limits->lower,
joint_urdf_->limits->upper,
error);
}
else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
{
error = angles::shortest_angular_distance(joint_.getPosition(), command);
}
else //prismatic
{
error = command - joint_.getPosition();
}
// Compute velocity error assuming desired velocity is 0
vel_error = 0.0 - joint_.getVelocity();
// Set the PID error and compute the PID command with nonuniform
// time step size. This also allows the user to pass in a precomputed derivative error.
double commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
joint_.setCommand(commanded_effort);
// publish state
if (loop_count_ % 10 == 0)
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command;
controller_state_publisher_->msg_.process_value = joint_.getPosition();
controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
controller_state_publisher_->msg_.error = error;
controller_state_publisher_->msg_.time_step = period.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
dummy);
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
}
void GLWaveformRendererFilteredSignal::draw(QPainter* painter, QPaintEvent* /*event*/) {
TrackPointer pTrack = m_waveformRenderer->getTrackInfo();
if (!pTrack) {
return;
}
ConstWaveformPointer waveform = pTrack->getWaveform();
if (waveform.isNull()) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
const int firstIndex = int(firstVisualIndex+0.5);
firstVisualIndex = firstIndex - firstIndex%2;
const int lastIndex = int(lastVisualIndex+0.5);
lastVisualIndex = lastIndex + lastIndex%2;
// Reset device for native painting
painter->beginNativePainting();
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// Per-band gain from the EQ knobs.
float allGain(1.0), lowGain(1.0), midGain(1.0), highGain(1.0);
getGains(&allGain, &lowGain, &midGain, &highGain);
float maxLow[2];
float maxMid[2];
float maxHigh[2];
float meanIndex;
// Removed Unsupported OpenGL functions, so that Mixxx can run on OpenGL-ES systems -- amvanbaren 9/2015
// TODO(XXX) Rewrite OpenGL code in glwaveformrendererfilteredsignal.cpp to support
// the new OpenGL syntax or use Qt instead
painter->endNativePainting();
}
void Pid::updateDynamicReconfig()
{
// Make sure dynamic reconfigure is initialized
if(!dynamic_reconfig_initialized_)
return;
// Get starting values
control_toolbox::ParametersConfig config;
// Get starting values
getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
updateDynamicReconfig(config);
}
void Pid::printValues()
{
Gains gains = getGains();
ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n"
<< " P Gain: " << gains.p_gain_ << "\n"
<< " I Gain: " << gains.i_gain_ << "\n"
<< " D Gain: " << gains.d_gain_ << "\n"
<< " I_Max: " << gains.i_max_ << "\n"
<< " I_Min: " << gains.i_min_ << "\n"
<< " P_Error_Last: " << p_error_last_ << "\n"
<< " P_Error: " << p_error_ << "\n"
<< " D_Error: " << d_error_ << "\n"
<< " I_Term: " << i_term_ << "\n"
<< " Command: " << cmd_
);
}
/* DEBUG: Implement publish? */
void JointPositionController::publish()
{
publisher_counter_++;
if (publisher_counter_ % publisher_rate_ == 0)
{
pr2_controllers_msgs::JointControllerStatePtr joint_controller_state_msg;
joint_controller_state_msg = controller_state_publisher_->allocate();
if (joint_controller_state_msg)
{
joint_controller_state_msg->header.stamp = robot_->getTime();
joint_controller_state_msg->set_point = command_;
joint_controller_state_msg->process_value = joint_state_->position_;
joint_controller_state_msg->process_value_dot = joint_state_->velocity_;
joint_controller_state_msg->error = error_;
joint_controller_state_msg->time_step = dt_.toSec();
joint_controller_state_msg->command = commanded_effort_;
double dummy;
getGains(joint_controller_state_msg->p, joint_controller_state_msg->i, joint_controller_state_msg->d, joint_controller_state_msg->i_clamp, dummy);
controller_state_publisher_->publish(joint_controller_state_msg);
}
}
}
void QtWaveformRendererSimpleSignal::draw(QPainter* painter, QPaintEvent* /*event*/){
TrackPointer pTrack = m_waveformRenderer->getTrackInfo();
if (!pTrack) {
return;
}
const Waveform* waveform = pTrack->getWaveform();
if (waveform == NULL) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
painter->save();
painter->setRenderHint(QPainter::Antialiasing);
painter->resetTransform();
float allGain(1.0);
getGains(&allGain, NULL, NULL, NULL);
double heightGain = allGain * (double)m_waveformRenderer->getHeight()/255.0;
if (m_alignment == Qt::AlignTop) {
painter->translate(0.0, 0.0);
painter->scale(1.0, heightGain);
} else if (m_alignment == Qt::AlignBottom) {
painter->translate(0.0, m_waveformRenderer->getHeight());
painter->scale(1.0, heightGain);
} else {
painter->translate(0.0, m_waveformRenderer->getHeight()/2.0);
painter->scale(1.0, 0.5*heightGain);
}
//draw reference line
if (m_alignment == Qt::AlignCenter) {
painter->setPen(m_pColors->getAxesColor());
painter->drawLine(0,0,m_waveformRenderer->getWidth(),0);
}
const double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
const double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
m_polygon.clear();
m_polygon.reserve(2 * m_waveformRenderer->getWidth() + 2);
m_polygon.append(QPointF(0.0, 0.0));
const double offset = firstVisualIndex;
// Represents the # of waveform data points per horizontal pixel.
const double gain = (lastVisualIndex - firstVisualIndex) /
(double)m_waveformRenderer->getWidth();
//NOTE(vrince) Please help me find a better name for "channelSeparation"
//this variable stand for merged channel ... 1 = merged & 2 = separated
int channelSeparation = 2;
if (m_alignment != Qt::AlignCenter)
channelSeparation = 1;
for (int channel = 0; channel < channelSeparation; ++channel) {
int startPixel = 0;
int endPixel = m_waveformRenderer->getWidth() - 1;
int delta = 1;
double direction = 1.0;
//Reverse display for merged bottom channel
if (m_alignment == Qt::AlignBottom)
direction = -1.0;
if (channel == 1) {
startPixel = m_waveformRenderer->getWidth() - 1;
endPixel = 0;
delta = -1;
direction = -1.0;
// After preparing the first channel, insert the pivot point.
m_polygon.append(QPointF(m_waveformRenderer->getWidth(), 0.0));
}
for (int x = startPixel;
(startPixel < endPixel) ? (x <= endPixel) : (x >= endPixel);
x += delta) {
// TODO(rryan) remove before 1.11 release. I'm seeing crashes
// sometimes where the pointIndex is very very large. It hasn't come
// back since adding locking, but I'm leaving this so that we can
// get some info about it before crashing. (The crash usually
// corrupts a lot of the stack).
if (m_polygon.size() > 2 * m_waveformRenderer->getWidth() + 2) {
qDebug() << "OUT OF CONTROL"
<< 2 * m_waveformRenderer->getWidth() + 2
<< dataSize
<< channel << m_polygon.size() << x;
}
// Width of the x position in visual indices.
const double xSampleWidth = gain * x;
// Effective visual index of x
const double xVisualSampleIndex = xSampleWidth + offset;
// Our current pixel (x) corresponds to a number of visual samples
// (visualSamplerPerPixel) in our waveform object. We take the max of
// all the data points on either side of xVisualSampleIndex within a
// window of 'maxSamplingRange' visual samples to measure the maximum
// data point contained by this pixel.
double maxSamplingRange = gain / 2.0;
// Since xVisualSampleIndex is in visual-samples (e.g. R,L,R,L) we want
// to check +/- maxSamplingRange frames, not samples. To do this, divide
// xVisualSampleIndex by 2. Since frames indices are integers, we round
// to the nearest integer by adding 0.5 before casting to int.
int visualFrameStart = int(xVisualSampleIndex / 2.0 - maxSamplingRange + 0.5);
int visualFrameStop = int(xVisualSampleIndex / 2.0 + maxSamplingRange + 0.5);
// If the entire sample range is off the screen then don't calculate a
// point for this pixel.
const int lastVisualFrame = dataSize / 2 - 1;
if (visualFrameStop < 0 || visualFrameStart > lastVisualFrame) {
m_polygon.append(QPointF(x, 0.0));
continue;
}
// We now know that some subset of [visualFrameStart,
// visualFrameStop] lies within the valid range of visual
// frames. Clamp visualFrameStart/Stop to within [0,
// lastVisualFrame].
visualFrameStart = math_max(math_min(lastVisualFrame, visualFrameStart), 0);
visualFrameStop = math_max(math_min(lastVisualFrame, visualFrameStop), 0);
int visualIndexStart = visualFrameStart * 2 + channel;
int visualIndexStop = visualFrameStop * 2 + channel;
// if (x == m_waveformRenderer->getWidth() / 2) {
// qDebug() << "audioVisualRatio" << waveform->getAudioVisualRatio();
// qDebug() << "visualSampleRate" << waveform->getVisualSampleRate();
// qDebug() << "audioSamplesPerVisualPixel" << waveform->getAudioSamplesPerVisualSample();
// qDebug() << "visualSamplePerPixel" << visualSamplePerPixel;
// qDebug() << "xSampleWidth" << xSampleWidth;
// qDebug() << "xVisualSampleIndex" << xVisualSampleIndex;
// qDebug() << "maxSamplingRange" << maxSamplingRange;;
// qDebug() << "Sampling pixel " << x << "over [" << visualIndexStart << visualIndexStop << "]";
// }
unsigned char maxAll = 0;
for (int i = visualIndexStart; i >= 0 && i < dataSize && i <= visualIndexStop;
i += channelSeparation) {
const WaveformData& waveformData = *(data + i);
unsigned char all = waveformData.filtered.all;
maxAll = math_max(maxAll, all);
}
m_polygon.append(QPointF(x, (float)maxAll * direction));
}
}
//If channel are not displayed separatly we nne to close the loop properly
if (channelSeparation == 1) {
m_polygon.append(QPointF(m_waveformRenderer->getWidth(), 0.0));
}
painter->setPen(m_borderPen);
painter->setBrush(m_brush);
painter->drawPolygon(&m_polygon[0], m_polygon.size());
painter->restore();
}
int QtWaveformRendererFilteredSignal::buildPolygon() {
// We have to check the track is present because it might have been unloaded
// between the call to draw and the call to buildPolygon
TrackPointer pTrack = m_waveformRenderer->getTrackInfo();
if (!pTrack) {
return 0;
}
const Waveform* waveform = pTrack->getWaveform();
if (waveform == NULL) {
return 0;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return 0;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return 0;
}
const double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
const double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
m_polygon[0].clear();
m_polygon[1].clear();
m_polygon[2].clear();
m_polygon[0].reserve(2 * m_waveformRenderer->getWidth() + 2);
m_polygon[1].reserve(2 * m_waveformRenderer->getWidth() + 2);
m_polygon[2].reserve(2 * m_waveformRenderer->getWidth() + 2);
QPointF point(0.0, 0.0);
m_polygon[0].append(point);
m_polygon[1].append(point);
m_polygon[2].append(point);
const double offset = firstVisualIndex;
// Represents the # of waveform data points per horizontal pixel.
const double gain = (lastVisualIndex - firstVisualIndex) /
(double)m_waveformRenderer->getWidth();
float lowGain(1.0), midGain(1.0), highGain(1.0);
getGains(NULL, &lowGain, &midGain, &highGain);
//NOTE(vrince) Please help me find a better name for "channelSeparation"
//this variable stand for merged channel ... 1 = merged & 2 = separated
int channelSeparation = 2;
if (m_alignment != Qt::AlignCenter)
channelSeparation = 1;
for (int channel = 0; channel < channelSeparation; ++channel) {
int startPixel = 0;
int endPixel = m_waveformRenderer->getWidth() - 1;
int delta = 1;
double direction = 1.0;
//Reverse display for merged bottom channel
if (m_alignment == Qt::AlignBottom)
direction = -1.0;
if (channel == 1) {
startPixel = m_waveformRenderer->getWidth() - 1;
endPixel = 0;
delta = -1;
direction = -1.0;
// After preparing the first channel, insert the pivot point.
point = QPointF(m_waveformRenderer->getWidth(), 0.0);
m_polygon[0].append(point);
m_polygon[1].append(point);
m_polygon[2].append(point);
}
for (int x = startPixel;
(startPixel < endPixel) ? (x <= endPixel) : (x >= endPixel);
x += delta) {
// TODO(rryan) remove before 1.11 release. I'm seeing crashes
// sometimes where the pointIndex is very very large. It hasn't come
// back since adding locking, but I'm leaving this so that we can
// get some info about it before crashing. (The crash usually
// corrupts a lot of the stack).
if (m_polygon[0].size() > 2 * m_waveformRenderer->getWidth() + 2) {
qDebug() << "OUT OF CONTROL"
<< 2 * m_waveformRenderer->getWidth() + 2
<< dataSize
<< channel << m_polygon[0].size() << x;
}
// Width of the x position in visual indices.
const double xSampleWidth = gain * x;
// Effective visual index of x
const double xVisualSampleIndex = xSampleWidth + offset;
// Our current pixel (x) corresponds to a number of visual samples
// (visualSamplerPerPixel) in our waveform object. We take the max of
// all the data points on either side of xVisualSampleIndex within a
// window of 'maxSamplingRange' visual samples to measure the maximum
// data point contained by this pixel.
double maxSamplingRange = gain / 2.0;
// Since xVisualSampleIndex is in visual-samples (e.g. R,L,R,L) we want
// to check +/- maxSamplingRange frames, not samples. To do this, divide
// xVisualSampleIndex by 2. Since frames indices are integers, we round
// to the nearest integer by adding 0.5 before casting to int.
int visualFrameStart = int(xVisualSampleIndex / 2.0 - maxSamplingRange + 0.5);
int visualFrameStop = int(xVisualSampleIndex / 2.0 + maxSamplingRange + 0.5);
// If the entire sample range is off the screen then don't calculate a
// point for this pixel.
const int lastVisualFrame = dataSize / 2 - 1;
if (visualFrameStop < 0 || visualFrameStart > lastVisualFrame) {
point = QPointF(x, 0.0);
m_polygon[0].append(point);
m_polygon[1].append(point);
m_polygon[2].append(point);
continue;
}
// We now know that some subset of [visualFrameStart,
// visualFrameStop] lies within the valid range of visual
// frames. Clamp visualFrameStart/Stop to within [0,
// lastVisualFrame].
visualFrameStart = math_max(math_min(lastVisualFrame, visualFrameStart), 0);
visualFrameStop = math_max(math_min(lastVisualFrame, visualFrameStop), 0);
int visualIndexStart = visualFrameStart * 2 + channel;
int visualIndexStop = visualFrameStop * 2 + channel;
// if (x == m_waveformRenderer->getWidth() / 2) {
// qDebug() << "audioVisualRatio" << waveform->getAudioVisualRatio();
// qDebug() << "visualSampleRate" << waveform->getVisualSampleRate();
// qDebug() << "audioSamplesPerVisualPixel" << waveform->getAudioSamplesPerVisualSample();
// qDebug() << "visualSamplePerPixel" << visualSamplePerPixel;
// qDebug() << "xSampleWidth" << xSampleWidth;
// qDebug() << "xVisualSampleIndex" << xVisualSampleIndex;
// qDebug() << "maxSamplingRange" << maxSamplingRange;;
// qDebug() << "Sampling pixel " << x << "over [" << visualIndexStart << visualIndexStop << "]";
// }
unsigned char maxLow = 0;
unsigned char maxBand = 0;
unsigned char maxHigh = 0;
for (int i = visualIndexStart; i >= 0 && i < dataSize && i <= visualIndexStop;
i += channelSeparation) {
const WaveformData& waveformData = *(data + i);
unsigned char low = waveformData.filtered.low;
unsigned char mid = waveformData.filtered.mid;
unsigned char high = waveformData.filtered.high;
maxLow = math_max(maxLow, low);
maxBand = math_max(maxBand, mid);
maxHigh = math_max(maxHigh, high);
}
m_polygon[0].append(QPointF(x, (float)maxLow * lowGain * direction));
m_polygon[1].append(QPointF(x, (float)maxBand * midGain * direction));
m_polygon[2].append(QPointF(x, (float)maxHigh * highGain * direction));
}
}
//If channel are not displayed separately we need to close the loop properly
if (channelSeparation == 1) {
point = QPointF(m_waveformRenderer->getWidth(), 0.0);
m_polygon[0].append(point);
m_polygon[1].append(point);
m_polygon[2].append(point);
}
return m_polygon[0].size();
}
void QtWaveformRendererFilteredSignal::draw(QPainter* painter, QPaintEvent* /*event*/) {
const TrackPointer pTrack = m_waveformRenderer->getTrackInfo();
if (!pTrack)
return;
painter->save();
painter->setRenderHint(QPainter::Antialiasing);
painter->resetTransform();
//visual gain
float allGain(1.0);
getGains(&allGain, NULL, NULL, NULL);
double heightGain = allGain * (double)m_waveformRenderer->getHeight()/255.0;
if (m_alignment == Qt::AlignTop) {
painter->translate(0.0, 0.0);
painter->scale(1.0, heightGain);
} else if (m_alignment == Qt::AlignBottom) {
painter->translate(0.0, m_waveformRenderer->getHeight());
painter->scale(1.0, heightGain);
} else {
painter->translate(0.0, m_waveformRenderer->getHeight()/2.0);
painter->scale(1.0, 0.5*heightGain);
}
//draw reference line
if (m_alignment == Qt::AlignCenter) {
painter->setPen(m_pColors->getAxesColor());
painter->drawLine(0,0,m_waveformRenderer->getWidth(),0);
}
int numberOfPoints = buildPolygon();
if (m_pLowKillControlObject && m_pLowKillControlObject->get() > 0.1) {
painter->setPen(QPen(m_lowKilledBrush, 0.0));
painter->setBrush(QColor(150,150,150,20));
} else {
painter->setPen(QPen(m_lowBrush, 0.0));
painter->setBrush(m_lowBrush);
}
painter->drawPolygon(&m_polygon[0][0], numberOfPoints);
if (m_pMidKillControlObject && m_pMidKillControlObject->get() > 0.1) {
painter->setPen(QPen(m_midKilledBrush, 0.0));
painter->setBrush(QColor(150,150,150,20));
} else {
painter->setPen(QPen(m_midBrush, 0.0));
painter->setBrush(m_midBrush);
}
painter->drawPolygon(&m_polygon[1][0], numberOfPoints);
if (m_pHighKillControlObject && m_pHighKillControlObject->get() > 0.1) {
painter->setPen(QPen(m_highKilledBrush, 0.0));
painter->setBrush(QColor(150,150,150,20));
} else {
painter->setPen(QPen(m_highBrush, 0.0));
painter->setBrush(m_highBrush);
}
painter->drawPolygon(&m_polygon[2][0], numberOfPoints);
painter->restore();
}
void WaveformRendererHSV::draw(QPainter* painter,
QPaintEvent* /*event*/) {
const TrackPointer trackInfo = m_waveformRenderer->getTrackInfo();
if (!trackInfo) {
return;
}
ConstWaveformPointer waveform = trackInfo->getWaveform();
if (waveform.isNull()) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
painter->save();
painter->setRenderHints(QPainter::Antialiasing, false);
painter->setRenderHints(QPainter::HighQualityAntialiasing, false);
painter->setRenderHints(QPainter::SmoothPixmapTransform, false);
painter->setWorldMatrixEnabled(false);
painter->resetTransform();
// Rotate if drawing vertical waveforms
if (m_waveformRenderer->getOrientation() == Qt::Vertical) {
painter->setTransform(QTransform(0, 1, 1, 0, 0, 0));
}
const double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
const double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
const double offset = firstVisualIndex;
// Represents the # of waveform data points per horizontal pixel.
const double gain = (lastVisualIndex - firstVisualIndex) /
(double)m_waveformRenderer->getLength();
float allGain(1.0);
getGains(&allGain, NULL, NULL, NULL);
// Save HSV of waveform color. NOTE(rryan): On ARM, qreal is float so it's
// important we use qreal here and not double or float or else we will get
// build failures on ARM.
qreal h, s, v;
// Get base color of waveform in the HSV format (s and v isn't use)
m_pColors->getLowColor().getHsvF(&h, &s, &v);
QColor color;
float lo, hi, total;
const int breadth = m_waveformRenderer->getBreadth();
const float halfBreadth = (float)breadth / 2.0;
const float heightFactor = allGain * halfBreadth / 255.0;
//draw reference line
painter->setPen(m_pColors->getAxesColor());
painter->drawLine(0, halfBreadth, m_waveformRenderer->getLength(), halfBreadth);
for (int x = 0; x < m_waveformRenderer->getLength(); ++x) {
// Width of the x position in visual indices.
const double xSampleWidth = gain * x;
// Effective visual index of x
const double xVisualSampleIndex = xSampleWidth + offset;
// Our current pixel (x) corresponds to a number of visual samples
// (visualSamplerPerPixel) in our waveform object. We take the max of
// all the data points on either side of xVisualSampleIndex within a
// window of 'maxSamplingRange' visual samples to measure the maximum
// data point contained by this pixel.
double maxSamplingRange = gain / 2.0;
// Since xVisualSampleIndex is in visual-samples (e.g. R,L,R,L) we want
// to check +/- maxSamplingRange frames, not samples. To do this, divide
// xVisualSampleIndex by 2. Since frames indices are integers, we round
// to the nearest integer by adding 0.5 before casting to int.
int visualFrameStart = int(xVisualSampleIndex / 2.0 - maxSamplingRange + 0.5);
int visualFrameStop = int(xVisualSampleIndex / 2.0 + maxSamplingRange + 0.5);
const int lastVisualFrame = dataSize / 2 - 1;
// We now know that some subset of [visualFrameStart, visualFrameStop]
// lies within the valid range of visual frames. Clamp
// visualFrameStart/Stop to within [0, lastVisualFrame].
visualFrameStart = math_clamp(visualFrameStart, 0, lastVisualFrame);
visualFrameStop = math_clamp(visualFrameStop, 0, lastVisualFrame);
int visualIndexStart = visualFrameStart * 2;
int visualIndexStop = visualFrameStop * 2;
int maxLow[2] = {0, 0};
int maxHigh[2] = {0, 0};
int maxMid[2] = {0, 0};
int maxAll[2] = {0, 0};
for (int i = visualIndexStart;
i >= 0 && i + 1 < dataSize && i + 1 <= visualIndexStop; i += 2) {
const WaveformData& waveformData = *(data + i);
const WaveformData& waveformDataNext = *(data + i + 1);
maxLow[0] = math_max(maxLow[0], (int)waveformData.filtered.low);
maxLow[1] = math_max(maxLow[1], (int)waveformDataNext.filtered.low);
maxMid[0] = math_max(maxMid[0], (int)waveformData.filtered.mid);
maxMid[1] = math_max(maxMid[1], (int)waveformDataNext.filtered.mid);
maxHigh[0] = math_max(maxHigh[0], (int)waveformData.filtered.high);
maxHigh[1] = math_max(maxHigh[1], (int)waveformDataNext.filtered.high);
maxAll[0] = math_max(maxAll[0], (int)waveformData.filtered.all);
maxAll[1] = math_max(maxAll[1], (int)waveformDataNext.filtered.all);
}
if (maxAll[0] && maxAll[1]) {
// Calculate sum, to normalize
// Also multiply on 1.2 to prevent very dark or light color
total = (maxLow[0] + maxLow[1] + maxMid[0] + maxMid[1] + maxHigh[0] + maxHigh[1]) * 1.2;
// prevent division by zero
if (total > 0)
{
// Normalize low and high (mid not need, because it not change the color)
lo = (maxLow[0] + maxLow[1]) / total;
hi = (maxHigh[0] + maxHigh[1]) / total;
}
else
lo = hi = 0.0;
// Set color
color.setHsvF(h, 1.0-hi, 1.0-lo);
painter->setPen(color);
switch (m_alignment) {
case Qt::AlignBottom :
case Qt::AlignRight :
painter->drawLine(
x, breadth,
x, breadth - (int)(heightFactor * (float)math_max(maxAll[0],maxAll[1])));
break;
case Qt::AlignTop :
case Qt::AlignLeft :
painter->drawLine(
x, 0,
x, (int)(heightFactor * (float)math_max(maxAll[0],maxAll[1])));
break;
default :
painter->drawLine(
x, (int)(halfBreadth - heightFactor * (float)maxAll[0]),
x, (int)(halfBreadth + heightFactor * (float)maxAll[1]));
}
}
}
painter->restore();
}
void GLWaveformRendererFilteredSignal::draw(QPainter* painter, QPaintEvent* /*event*/) {
TrackPointer pTrack = m_waveformRenderer->getTrackInfo();
if (!pTrack) {
return;
}
ConstWaveformPointer waveform = pTrack->getWaveform();
if (waveform.isNull()) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
const double lineWidth = (1.0 / m_waveformRenderer->getVisualSamplePerPixel()) + 1.0;
const int firstIndex = int(firstVisualIndex+0.5);
firstVisualIndex = firstIndex - firstIndex%2;
const int lastIndex = int(lastVisualIndex+0.5);
lastVisualIndex = lastIndex + lastIndex%2;
// Reset device for native painting
painter->beginNativePainting();
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// Per-band gain from the EQ knobs.
float allGain(1.0), lowGain(1.0), midGain(1.0), highGain(1.0);
getGains(&allGain, &lowGain, &midGain, &highGain);
#ifndef __OPENGLES__
if (m_alignment == Qt::AlignCenter) {
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
if (m_orientation == Qt::Vertical) {
glRotatef(90.0f, 0.0f, 0.0f, 1.0f);
glScalef(-1.0f, 1.0f, 1.0f);
}
glOrtho(firstVisualIndex, lastVisualIndex, -255.0, 255.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glScalef(1.f,allGain,1.f);
glLineWidth(1.0);
glDisable(GL_LINE_SMOOTH);
//draw reference line
glBegin(GL_LINES); {
glColor4f(m_axesColor_r, m_axesColor_g,
m_axesColor_b, m_axesColor_a);
glVertex2f(firstVisualIndex,0);
glVertex2f(lastVisualIndex,0);
}
glEnd();
glLineWidth(lineWidth);
glEnable(GL_LINE_SMOOTH);
glBegin(GL_LINES); {
int firstIndex = math_max(static_cast<int>(firstVisualIndex), 0);
int lastIndex = math_min(static_cast<int>(lastVisualIndex), dataSize);
glColor4f(m_lowColor_r, m_lowColor_g, m_lowColor_b, 0.8);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxLow0 = data[visualIndex].filtered.low;
GLfloat maxLow1 = data[visualIndex+1].filtered.low;
glVertex2f(visualIndex,lowGain*maxLow0);
glVertex2f(visualIndex,-1.f*lowGain*maxLow1);
}
glColor4f(m_midColor_r, m_midColor_g, m_midColor_b, 0.85);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxMid0 = data[visualIndex].filtered.mid;
GLfloat maxMid1 = data[visualIndex+1].filtered.mid;
glVertex2f(visualIndex, midGain * maxMid0);
glVertex2f(visualIndex,-1.f * midGain * maxMid1);
}
glColor4f(m_highColor_r, m_highColor_g, m_highColor_b, 0.9);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxHigh0 = data[visualIndex].filtered.high;
GLfloat maxHigh1 = data[visualIndex + 1].filtered.high;
glVertex2f(visualIndex, highGain * maxHigh0);
glVertex2f(visualIndex, -1.f * highGain * maxHigh1);
}
}
glEnd();
} else { //top || bottom
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
if (m_orientation == Qt::Vertical) {
glRotatef(90.0f, 0.0f, 0.0f, 1.0f);
glScalef(-1.0f, 1.0f, 1.0f);
}
if (m_alignment == Qt::AlignBottom || m_alignment == Qt::AlignRight)
glOrtho(firstVisualIndex, lastVisualIndex, 0.0, 255.0, -10.0, 10.0);
else
glOrtho(firstVisualIndex, lastVisualIndex, 255.0, 0.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glScalef(1.f,allGain,1.f);
glLineWidth(lineWidth);
glEnable(GL_LINE_SMOOTH);
glBegin(GL_LINES); {
int firstIndex = math_max(static_cast<int>(firstVisualIndex), 0);
int lastIndex = math_min(static_cast<int>(lastVisualIndex), dataSize);
glColor4f(m_lowColor_r, m_lowColor_g, m_lowColor_b, 0.8);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxLow = math_max(
data[visualIndex].filtered.low,
data[visualIndex+1].filtered.low);
glVertex2f(visualIndex, 0);
glVertex2f(visualIndex, lowGain * maxLow);
}
glColor4f(m_midColor_r, m_midColor_g, m_midColor_b, 0.85);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxMid = math_max(
data[visualIndex].filtered.mid,
data[visualIndex+1].filtered.mid);
glVertex2f(visualIndex, 0.f);
glVertex2f(visualIndex, midGain * maxMid);
}
glColor4f(m_highColor_r, m_highColor_g, m_highColor_b, 0.9);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxHigh = math_max(
data[visualIndex].filtered.high,
data[visualIndex + 1].filtered.high);
glVertex2f(visualIndex, 0.f);
glVertex2f(visualIndex, highGain * maxHigh);
}
}
glEnd();
}
//DEBUG
/*glDisable(GL_ALPHA_TEST);
glBegin(GL_LINE_LOOP);
{
glColor4f(0.5,1.0,0.5,0.25);
glVertex3f(firstVisualIndex,-1.0f, 0.0f);
glVertex3f(lastVisualIndex, 1.0f, 0.0f);
glVertex3f(lastVisualIndex,-1.0f, 0.0f);
glVertex3f(firstVisualIndex, 1.0f, 0.0f);
}
glEnd();*/
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
#endif
painter->endNativePainting();
}
void GLWaveformRendererRGB::draw(QPainter* painter, QPaintEvent* /*event*/) {
TrackPointer pTrack = m_waveformRenderer->getTrackInfo();
if (!pTrack) {
return;
}
const Waveform* waveform = pTrack->getWaveform();
if (waveform == NULL) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
const int firstIndex = int(firstVisualIndex + 0.5);
firstVisualIndex = firstIndex - firstIndex % 2;
const int lastIndex = int(lastVisualIndex + 0.5);
lastVisualIndex = lastIndex + lastIndex % 2;
// Reset device for native painting
painter->beginNativePainting();
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// Per-band gain from the EQ knobs.
float allGain(1.0), lowGain(1.0), midGain(1.0), highGain(1.0);
getGains(&allGain, &lowGain, &midGain, &highGain);
if (m_alignment == Qt::AlignCenter) {
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(firstVisualIndex, lastVisualIndex, -255.0, 255.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glScalef(1.0f, allGain, 1.0f);
glLineWidth(1.2);
glDisable(GL_LINE_SMOOTH);
// Draw reference line
glBegin(GL_LINES); {
glColor4f(m_axesColor_r, m_axesColor_g, m_axesColor_b, m_axesColor_a);
glVertex2f(firstVisualIndex, 0);
glVertex2f(lastVisualIndex, 0);
}
glEnd();
glLineWidth(1.2);
glEnable(GL_LINE_SMOOTH);
glBegin(GL_LINES); {
for( int visualIndex = firstVisualIndex;
visualIndex < lastVisualIndex;
visualIndex += 2) {
if( visualIndex < 0)
continue;
if( visualIndex > dataSize - 1)
break;
float left_low = lowGain * (float) data[visualIndex].filtered.low;
float left_mid = midGain * (float) data[visualIndex].filtered.mid;
float left_high = highGain * (float) data[visualIndex].filtered.high;
float left_all = sqrtf(left_low * left_low + left_mid * left_mid + left_high * left_high);
float left_red = left_low * m_lowColor.red() + left_mid * m_midColor.red() + left_high * m_highColor.red();
float left_green = left_low * m_lowColor.green() + left_mid * m_midColor.green() + left_high * m_highColor.green();
float left_blue = left_low * m_lowColor.blue() + left_mid * m_midColor.blue() + left_high * m_highColor.blue();
float left_max = math_max3(left_red, left_green, left_blue);
if (left_max > 0.0f) { // Prevent division by zero
glColor4f(left_red / left_max, left_green / left_max, left_blue / left_max, 0.8f);
glVertex2f(visualIndex, 0.0f);
glVertex2f(visualIndex, left_all);
}
float right_low = lowGain * (float) data[visualIndex+1].filtered.low;
float right_mid = midGain * (float) data[visualIndex+1].filtered.mid;
float right_high = highGain * (float) data[visualIndex+1].filtered.high;
float right_all = sqrtf(right_low * right_low + right_mid * right_mid + right_high * right_high);
float right_red = right_low * m_lowColor.red() + right_mid * m_midColor.red() + right_high * m_highColor.red();
float right_green = right_low * m_lowColor.green() + right_mid * m_midColor.green() + right_high * m_highColor.green();
float right_blue = right_low * m_lowColor.blue() + right_mid * m_midColor.blue() + right_high * m_highColor.blue();
float right_max = math_max3(right_red, right_green, right_blue);
if (right_max > 0.0f) { // Prevent division by zero
glColor4f(right_red / right_max, right_green / right_max, right_blue / right_max, 0.8f);
glVertex2f(visualIndex, 0.0f);
glVertex2f(visualIndex, -1.0f * right_all);
}
}
}
glEnd();
} else { // top || bottom
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
if( m_alignment == Qt::AlignBottom)
glOrtho(firstVisualIndex, lastVisualIndex, 0.0, 255.0, -10.0, 10.0);
else
glOrtho(firstVisualIndex, lastVisualIndex, 255.0, 0.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glScalef(1.0f, allGain, 1.0f);
glLineWidth(1.2);
glEnable(GL_LINE_SMOOTH);
glBegin(GL_LINES); {
for( int visualIndex = firstVisualIndex;
visualIndex < lastVisualIndex;
visualIndex += 2) {
if( visualIndex < 0)
continue;
if( visualIndex > dataSize - 1)
break;
float low = lowGain * (float) math_max(data[visualIndex].filtered.low, data[visualIndex+1].filtered.low);
float mid = midGain * (float) math_max(data[visualIndex].filtered.mid, data[visualIndex+1].filtered.mid);
float high = highGain * (float) math_max(data[visualIndex].filtered.high, data[visualIndex+1].filtered.high);
float all = sqrtf(low * low + mid * mid + high * high);
float red = low * m_lowColor.red() + mid * m_midColor.red() + high * m_highColor.red();
float green = low * m_lowColor.green() + mid * m_midColor.green() + high * m_highColor.green();
float blue = low * m_lowColor.blue() + mid * m_midColor.blue() + high * m_highColor.blue();
float max = math_max3(red, green, blue);
if (max > 0.0f) { // Prevent division by zero
glColor4f(red / max, green / max, blue / max, 0.9f);
glVertex2f(float(visualIndex), 0.0f);
glVertex2f(float(visualIndex), all);
}
}
}
glEnd();
}
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
painter->endNativePainting();
}
void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
{
command_struct_ = *(command_.readFromRT());
double command_position = command_struct_.position_;
double command_velocity = command_struct_.velocity_;
bool has_velocity_ = command_struct_.has_velocity_;
double error, vel_error;
double commanded_effort;
double current_position = joint_.getPosition();
// Make sure joint is within limits if applicable
enforceJointLimits(command_position);
// Compute position error
if (joint_urdf_->type == urdf::Joint::REVOLUTE)
{
angles::shortest_angular_distance_with_limits(
current_position,
command_position,
joint_urdf_->limits->lower,
joint_urdf_->limits->upper,
error);
}
else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
{
error = angles::shortest_angular_distance(current_position, command_position);
}
else //prismatic
{
error = command_position - current_position;
}
// Decide which of the two PID computeCommand() methods to call
if (has_velocity_)
{
// Compute velocity error if a non-zero velocity command was given
vel_error = command_velocity - joint_.getVelocity();
// Set the PID error and compute the PID command with nonuniform
// time step size. This also allows the user to pass in a precomputed derivative error.
commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
}
else
{
// Set the PID error and compute the PID command with nonuniform
// time step size.
commanded_effort = pid_controller_.computeCommand(error, period);
}
joint_.setCommand(commanded_effort);
// publish state
if (loop_count_ % 10 == 0)
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command_position;
controller_state_publisher_->msg_.process_value = current_position;
controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
controller_state_publisher_->msg_.error = error;
controller_state_publisher_->msg_.time_step = period.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
bool antiwindup;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
dummy,
antiwindup);
controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
}
void WaveformRendererFilteredSignal::draw(QPainter* painter,
QPaintEvent* /*event*/) {
const TrackPointer trackInfo = m_waveformRenderer->getTrackInfo();
if (!trackInfo) {
return;
}
const Waveform* waveform = trackInfo->getWaveform();
if (waveform == NULL) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
painter->save();
painter->setRenderHints(QPainter::Antialiasing, false);
painter->setRenderHints(QPainter::HighQualityAntialiasing, false);
painter->setRenderHints(QPainter::SmoothPixmapTransform, false);
painter->setWorldMatrixEnabled(false);
painter->resetTransform();
const double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
const double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
// Represents the # of waveform data points per horizontal pixel.
const double gain = (lastVisualIndex - firstVisualIndex) /
(double)m_waveformRenderer->getWidth();
// Per-band gain from the EQ knobs.
float allGain(1.0), lowGain(1.0), midGain(1.0), highGain(1.0);
getGains(&allGain, &lowGain, &midGain, &highGain);
const float halfHeight = (float)m_waveformRenderer->getHeight()/2.0;
const float heightFactor = m_alignment == Qt::AlignCenter
? allGain*halfHeight/255.0
: allGain*m_waveformRenderer->getHeight()/255.0;
//draw reference line
if (m_alignment == Qt::AlignCenter) {
painter->setPen(m_pColors->getAxesColor());
painter->drawLine(0,halfHeight,m_waveformRenderer->getWidth(),halfHeight);
}
int actualLowLineNumber = 0;
int actualMidLineNumber = 0;
int actualHighLineNumber = 0;
for (int x = 0; x < m_waveformRenderer->getWidth(); ++x) {
// Width of the x position in visual indices.
const double xSampleWidth = gain * x;
// Effective visual index of x
const double xVisualSampleIndex = xSampleWidth + firstVisualIndex;
// Our current pixel (x) corresponds to a number of visual samples
// (visualSamplerPerPixel) in our waveform object. We take the max of
// all the data points on either side of xVisualSampleIndex within a
// window of 'maxSamplingRange' visual samples to measure the maximum
// data point contained by this pixel.
double maxSamplingRange = gain / 2.0;
// Since xVisualSampleIndex is in visual-samples (e.g. R,L,R,L) we want
// to check +/- maxSamplingRange frames, not samples. To do this, divide
// xVisualSampleIndex by 2. Since frames indices are integers, we round
// to the nearest integer by adding 0.5 before casting to int.
int visualFrameStart = int(xVisualSampleIndex / 2.0 - maxSamplingRange + 0.5);
int visualFrameStop = int(xVisualSampleIndex / 2.0 + maxSamplingRange + 0.5);
// If the entire sample range is off the screen then don't calculate a
// point for this pixel.
const int lastVisualFrame = dataSize / 2 - 1;
if (visualFrameStop < 0 || visualFrameStart > lastVisualFrame) {
continue;
}
// We now know that some subset of [visualFrameStart, visualFrameStop]
// lies within the valid range of visual frames. Clamp
// visualFrameStart/Stop to within [0, lastVisualFrame].
visualFrameStart = math_clamp(visualFrameStart, 0, lastVisualFrame);
visualFrameStop = math_clamp(visualFrameStop, 0, lastVisualFrame);
int visualIndexStart = visualFrameStart * 2;
int visualIndexStop = visualFrameStop * 2;
// if (x == m_waveformRenderer->getWidth() / 2) {
// qDebug() << "audioVisualRatio" << waveform->getAudioVisualRatio();
// qDebug() << "visualSampleRate" << waveform->getVisualSampleRate();
// qDebug() << "audioSamplesPerVisualPixel" << waveform->getAudioSamplesPerVisualSample();
// qDebug() << "visualSamplePerPixel" << visualSamplePerPixel;
// qDebug() << "xSampleWidth" << xSampleWidth;
// qDebug() << "xVisualSampleIndex" << xVisualSampleIndex;
// qDebug() << "maxSamplingRange" << maxSamplingRange;;
// qDebug() << "Sampling pixel " << x << "over [" << visualIndexStart << visualIndexStop << "]";
// }
unsigned char maxLow[2] = {0, 0};
unsigned char maxMid[2] = {0, 0};
unsigned char maxHigh[2] = {0, 0};
for (int i = visualIndexStart;
i >= 0 && i + 1 < dataSize && i + 1 <= visualIndexStop; i += 2) {
const WaveformData& waveformData = *(data + i);
const WaveformData& waveformDataNext = *(data + i + 1);
maxLow[0] = math_max(maxLow[0], waveformData.filtered.low);
maxLow[1] = math_max(maxLow[1], waveformDataNext.filtered.low);
maxMid[0] = math_max(maxMid[0], waveformData.filtered.mid);
maxMid[1] = math_max(maxMid[1], waveformDataNext.filtered.mid);
maxHigh[0] = math_max(maxHigh[0], waveformData.filtered.high);
maxHigh[1] = math_max(maxHigh[1], waveformDataNext.filtered.high);
}
if (maxLow[0] && maxLow[1]) {
switch (m_alignment) {
case Qt::AlignBottom :
m_lowLines[actualLowLineNumber].setLine(
x, m_waveformRenderer->getHeight(),
x, m_waveformRenderer->getHeight() - (int)(heightFactor*lowGain*(float)math_max(maxLow[0],maxLow[1])));
break;
case Qt::AlignTop :
m_lowLines[actualLowLineNumber].setLine(
x, 0,
x, (int)(heightFactor*lowGain*(float)math_max(maxLow[0],maxLow[1])));
break;
default :
m_lowLines[actualLowLineNumber].setLine(
x, (int)(halfHeight-heightFactor*(float)maxLow[0]*lowGain),
x, (int)(halfHeight+heightFactor*(float)maxLow[1]*lowGain));
break;
}
actualLowLineNumber++;
}
if (maxMid[0] && maxMid[1]) {
switch (m_alignment) {
case Qt::AlignBottom :
m_midLines[actualMidLineNumber].setLine(
x, m_waveformRenderer->getHeight(),
x, m_waveformRenderer->getHeight() - (int)(heightFactor*midGain*(float)math_max(maxMid[0],maxMid[1])));
break;
case Qt::AlignTop :
m_midLines[actualMidLineNumber].setLine(
x, 0,
x, (int)(heightFactor*midGain*(float)math_max(maxMid[0],maxMid[1])));
break;
default :
m_midLines[actualMidLineNumber].setLine(
x, (int)(halfHeight-heightFactor*(float)maxMid[0]*midGain),
x, (int)(halfHeight+heightFactor*(float)maxMid[1]*midGain));
break;
}
actualMidLineNumber++;
}
if (maxHigh[0] && maxHigh[1]) {
switch (m_alignment) {
case Qt::AlignBottom :
m_highLines[actualHighLineNumber].setLine(
x, m_waveformRenderer->getHeight(),
x, m_waveformRenderer->getHeight() - (int)(heightFactor*highGain*(float)math_max(maxHigh[0],maxHigh[1])));
break;
case Qt::AlignTop :
m_highLines[actualHighLineNumber].setLine(
x, 0,
x, (int)(heightFactor*highGain*(float)math_max(maxHigh[0],maxHigh[1])));
break;
default :
m_highLines[actualHighLineNumber].setLine(
x, (int)(halfHeight-heightFactor*(float)maxHigh[0]*highGain),
x, (int)(halfHeight+heightFactor*(float)maxHigh[1]*highGain));
break;
}
actualHighLineNumber++;
}
}
painter->setPen(QPen(QBrush(m_pColors->getLowColor()), 1));
if (m_pLowKillControlObject && m_pLowKillControlObject->get() == 0.0) {
painter->drawLines(&m_lowLines[0], actualLowLineNumber);
}
painter->setPen(QPen(QBrush(m_pColors->getMidColor()), 1));
if (m_pMidKillControlObject && m_pMidKillControlObject->get() == 0.0) {
painter->drawLines(&m_midLines[0], actualMidLineNumber);
}
painter->setPen(QPen(QBrush(m_pColors->getHighColor()), 1));
if (m_pHighKillControlObject && m_pHighKillControlObject->get() == 0.0) {
painter->drawLines(&m_highLines[0], actualHighLineNumber);
}
painter->restore();
}
void GLSLWaveformRendererSignal::draw(QPainter* painter, QPaintEvent* /*event*/) {
if (!m_frameBuffersValid || !m_shadersValid) {
return;
}
TrackPointer trackInfo = m_waveformRenderer->getTrackInfo();
if (!trackInfo) {
return;
}
ConstWaveformPointer waveform = trackInfo->getWaveform();
if (waveform.isNull()) {
return;
}
int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
// save the GL state set for QPainter
painter->beginNativePainting();
//NOTE: (vRince) completion can change during loadTexture
//do not remove currenCompletion temp variable !
const int currentCompletion = waveform->getCompletion();
if (m_loadedWaveform < currentCompletion) {
loadTexture();
m_loadedWaveform = currentCompletion;
}
// Per-band gain from the EQ knobs.
float lowGain(1.0), midGain(1.0), highGain(1.0), allGain(1.0);
getGains(&allGain, &lowGain, &midGain, &highGain);
double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize/2.0;
double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize/2.0;
// const int firstIndex = int(firstVisualIndex+0.5);
// firstVisualIndex = firstIndex - firstIndex%2;
// const int lastIndex = int(lastVisualIndex+0.5);
// lastVisualIndex = lastIndex + lastIndex%2;
//qDebug() << "GAIN" << allGain << lowGain << midGain << highGain;
//paint into frame buffer
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(firstVisualIndex, lastVisualIndex, -1.0, 1.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glTranslatef(.0f,.0f,.0f);
m_frameShaderProgram->bind();
glViewport(0, 0, m_framebuffer->width(), m_framebuffer->height());
m_frameShaderProgram->setUniformValue("framebufferSize", QVector2D(
m_framebuffer->width(), m_framebuffer->height()));
m_frameShaderProgram->setUniformValue("waveformLength", dataSize);
m_frameShaderProgram->setUniformValue("textureSize", waveform->getTextureSize());
m_frameShaderProgram->setUniformValue("textureStride", waveform->getTextureStride());
m_frameShaderProgram->setUniformValue("firstVisualIndex", (float)firstVisualIndex);
m_frameShaderProgram->setUniformValue("lastVisualIndex", (float)lastVisualIndex);
m_frameShaderProgram->setUniformValue("allGain", allGain);
m_frameShaderProgram->setUniformValue("lowGain", lowGain);
m_frameShaderProgram->setUniformValue("midGain", midGain);
m_frameShaderProgram->setUniformValue("highGain", highGain);
m_frameShaderProgram->setUniformValue("axesColor", QVector4D(m_axesColor_r, m_axesColor_g,
m_axesColor_b, m_axesColor_a));
QVector4D lowColor = m_rgbShader ?
QVector4D(m_rgbLowColor_r, m_rgbLowColor_g, m_rgbLowColor_b, 1.0) :
QVector4D(m_lowColor_r, m_lowColor_g, m_lowColor_b, 1.0);
QVector4D midColor = m_rgbShader ?
QVector4D(m_rgbMidColor_r, m_rgbMidColor_g, m_rgbMidColor_b, 1.0) :
QVector4D(m_midColor_r, m_midColor_g, m_midColor_b, 1.0);
QVector4D highColor = m_rgbShader ?
QVector4D(m_rgbHighColor_r, m_rgbHighColor_g, m_rgbHighColor_b, 1.0) :
QVector4D(m_highColor_r, m_highColor_g, m_highColor_b, 1.0);
m_frameShaderProgram->setUniformValue("lowColor", lowColor);
m_frameShaderProgram->setUniformValue("midColor", midColor);
m_frameShaderProgram->setUniformValue("highColor", highColor);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, m_textureId);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
m_framebuffer->bind();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//glCallList(m_unitQuadListId);
glBegin(GL_QUADS);
{
glTexCoord2f(0.0,0.0);
glVertex3f(firstVisualIndex, -1.0f, 0.0f);
glTexCoord2f(1.0, 0.0);
glVertex3f(lastVisualIndex, -1.0f, 0.0f);
glTexCoord2f(1.0,1.0);
glVertex3f(lastVisualIndex, 1.0f, 0.0f);
glTexCoord2f(0.0,1.0);
glVertex3f(firstVisualIndex, 1.0f, 0.0f);
}
glEnd();
m_framebuffer->release();
m_frameShaderProgram->release();
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
if (m_bDumpPng) {
m_framebuffer->toImage().save("m_framebuffer.png");
m_bDumpPng = false;
}
}
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(-1.0, 1.0, -1.0, 1.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glTranslatef(0.0, 0.0, 0.0);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_TEXTURE_2D);
//paint buffer into viewport
{
glViewport(0, 0, m_waveformRenderer->getWidth(), m_waveformRenderer->getHeight());
glBindTexture(GL_TEXTURE_2D, m_framebuffer->texture());
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glBegin(GL_QUADS);
{
glTexCoord2f(0.0, 0.0);
glVertex3f(-1.0f, -1.0f, 0.0f);
glTexCoord2f(1.0, 0.0);
glVertex3f(1.0f, -1.0f, 0.0f);
glTexCoord2f(1.0, 1.0);
glVertex3f(1.0f, 1.0f, 0.0f);
glTexCoord2f(0.0, 1.0);
glVertex3f(-1.0f, 1.0f, 0.0f);
}
glEnd();
}
glDisable(GL_TEXTURE_2D);
glDisable(GL_ALPHA_TEST);
//DEBUG
/*
glBegin(GL_LINE_LOOP);
{
glColor4f(0.5,1.0,0.5,0.75);
glVertex3f(-1.0f,-1.0f, 0.0f);
glVertex3f(1.0f, 1.0f, 0.0f);
glVertex3f(1.0f,-1.0f, 0.0f);
glVertex3f(-1.0f, 1.0f, 0.0f);
}
glEnd();
*/
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
painter->endNativePainting();
}
void WaveformRendererRGB::draw(QPainter* painter,
QPaintEvent* /*event*/) {
const TrackPointer trackInfo = m_waveformRenderer->getTrackInfo();
if (!trackInfo) {
return;
}
const Waveform* waveform = trackInfo->getWaveform();
if (waveform == NULL) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
painter->save();
painter->setRenderHints(QPainter::Antialiasing, false);
painter->setRenderHints(QPainter::HighQualityAntialiasing, false);
painter->setRenderHints(QPainter::SmoothPixmapTransform, false);
painter->setWorldMatrixEnabled(false);
painter->resetTransform();
const double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
const double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
const double offset = firstVisualIndex;
// Represents the # of waveform data points per horizontal pixel.
const double gain = (lastVisualIndex - firstVisualIndex) /
(double)m_waveformRenderer->getWidth();
float allGain(1.0);
getGains(&allGain, NULL, NULL, NULL);
QColor color;
const float halfHeight = (float)m_waveformRenderer->getHeight()/2.0;
const float heightFactor = allGain*halfHeight/255.0;
// Draw reference line
painter->setPen(m_pColors->getAxesColor());
painter->drawLine(0,halfHeight,m_waveformRenderer->getWidth(),halfHeight);
for (int x = 0; x < m_waveformRenderer->getWidth(); ++x) {
// Width of the x position in visual indices.
const double xSampleWidth = gain * x;
// Effective visual index of x
const double xVisualSampleIndex = xSampleWidth + offset;
// Our current pixel (x) corresponds to a number of visual samples
// (visualSamplerPerPixel) in our waveform object. We take the max of
// all the data points on either side of xVisualSampleIndex within a
// window of 'maxSamplingRange' visual samples to measure the maximum
// data point contained by this pixel.
double maxSamplingRange = gain / 2.0;
// Since xVisualSampleIndex is in visual-samples (e.g. R,L,R,L) we want
// to check +/- maxSamplingRange frames, not samples. To do this, divide
// xVisualSampleIndex by 2. Since frames indices are integers, we round
// to the nearest integer by adding 0.5 before casting to int.
int visualFrameStart = int(xVisualSampleIndex / 2.0 - maxSamplingRange + 0.5);
int visualFrameStop = int(xVisualSampleIndex / 2.0 + maxSamplingRange + 0.5);
const int lastVisualFrame = dataSize / 2 - 1;
// We now know that some subset of [visualFrameStart, visualFrameStop]
// lies within the valid range of visual frames. Clamp
// visualFrameStart/Stop to within [0, lastVisualFrame].
visualFrameStart = math_clamp(visualFrameStart, 0, lastVisualFrame);
visualFrameStop = math_clamp(visualFrameStop, 0, lastVisualFrame);
int visualIndexStart = visualFrameStart * 2;
int visualIndexStop = visualFrameStop * 2;
unsigned char maxLow = 0;
unsigned char maxMid = 0;
unsigned char maxHigh = 0;
unsigned char maxAllA = 0;
unsigned char maxAllB = 0;
for (int i = visualIndexStart;
i >= 0 && i + 1 < dataSize && i + 1 <= visualIndexStop; i += 2) {
const WaveformData& waveformData = *(data + i);
const WaveformData& waveformDataNext = *(data + i + 1);
maxLow = math_max3(maxLow, waveformData.filtered.low, waveformDataNext.filtered.low);
maxMid = math_max3(maxMid, waveformData.filtered.mid, waveformDataNext.filtered.mid);
maxHigh = math_max3(maxHigh, waveformData.filtered.high, waveformDataNext.filtered.high);
maxAllA = math_max(maxAllA, waveformData.filtered.all);
maxAllB = math_max(maxAllB, waveformDataNext.filtered.all);
}
int red = maxLow * m_lowColor.red() + maxMid * m_midColor.red() + maxHigh * m_highColor.red();
int green = maxLow * m_lowColor.green() + maxMid * m_midColor.green() + maxHigh * m_highColor.green();
int blue = maxLow * m_lowColor.blue() + maxMid * m_midColor.blue() + maxHigh * m_highColor.blue();
// Compute maximum (needed for value normalization)
float max = (float) math_max3(red, green, blue);
// Prevent division by zero
if (max > 0.0f) {
// Set color
color.setRgbF(red / max, green / max, blue / max);
painter->setPen(color);
switch (m_alignment) {
case Qt::AlignBottom :
painter->drawLine(
x, m_waveformRenderer->getHeight(),
x, m_waveformRenderer->getHeight() - (int)(heightFactor*(float)math_max(maxAllA,maxAllB)));
break;
case Qt::AlignTop :
painter->drawLine(
x, 0,
x, (int)(heightFactor*(float)math_max(maxAllA,maxAllB)));
break;
default :
painter->drawLine(
x, (int)(halfHeight-heightFactor*(float)maxAllA),
x, (int)(halfHeight+heightFactor*(float)maxAllB));
}
}
}
painter->restore();
}
void GLWaveformRendererSimpleSignal::draw(QPainter* painter, QPaintEvent* /*event*/) {
TrackPointer pTrack = m_waveformRenderer->getTrackInfo();
if (!pTrack) {
return;
}
ConstWaveformPointer waveform = pTrack->getWaveform();
if (waveform.isNull()) {
return;
}
const int dataSize = waveform->getDataSize();
if (dataSize <= 1) {
return;
}
const WaveformData* data = waveform->data();
if (data == NULL) {
return;
}
double firstVisualIndex = m_waveformRenderer->getFirstDisplayedPosition() * dataSize;
double lastVisualIndex = m_waveformRenderer->getLastDisplayedPosition() * dataSize;
double lineWidth = (1.0 / m_waveformRenderer->getVisualSamplePerPixel()) + 1.0;
const int firstIndex = int(firstVisualIndex+0.5);
firstVisualIndex = firstIndex - firstIndex%2;
const int lastIndex = int(lastVisualIndex+0.5);
lastVisualIndex = lastIndex + lastIndex%2;
// Reset device for native painting
painter->beginNativePainting();
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
float allGain(1.0);
getGains(&allGain, NULL, NULL, NULL);
if (m_alignment == Qt::AlignCenter) {
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
if (m_orientation == Qt::Vertical) {
glRotatef(90.0f, 0.0f, 0.0f, 1.0f);
glScalef(-1.0f, 1.0f, 1.0f);
}
glOrtho(firstVisualIndex, lastVisualIndex, -255.0, 255.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glScalef(1.f, allGain, 1.f);
glLineWidth(1.0);
glDisable(GL_LINE_SMOOTH);
//draw reference line
glBegin(GL_LINES); {
glColor4f(m_axesColor_r, m_axesColor_g,
m_axesColor_b, m_axesColor_a);
glVertex2f(firstVisualIndex,0);
glVertex2f(lastVisualIndex,0);
}
glEnd();
glLineWidth(lineWidth);
glEnable(GL_LINE_SMOOTH);
glBegin(GL_LINES); {
int firstIndex = math_max(static_cast<int>(firstVisualIndex), 0);
int lastIndex = math_min(static_cast<int>(lastVisualIndex), dataSize);
glColor4f(m_signalColor_r, m_signalColor_g, m_signalColor_b, 0.9);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxAll0 = data[visualIndex].filtered.all;
GLfloat maxAll1 = data[visualIndex+1].filtered.all;
glVertex2f(visualIndex, maxAll0);
glVertex2f(visualIndex, -1.f * maxAll1);
}
}
glEnd();
} else { //top || bottom
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
if (m_orientation == Qt::Vertical) {
glRotatef(90.0f, 0.0f, 0.0f, 1.0f);
glScalef(-1.0f, 1.0f, 1.0f);
}
if (m_alignment == Qt::AlignBottom || m_alignment == Qt::AlignRight)
glOrtho(firstVisualIndex, lastVisualIndex, 0.0, 255.0, -10.0, 10.0);
else
glOrtho(firstVisualIndex, lastVisualIndex, 255.0, 0.0, -10.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glScalef(1.f, allGain, 1.f);
glLineWidth(lineWidth);
glEnable(GL_LINE_SMOOTH);
glBegin(GL_LINES); {
int firstIndex = math_max(static_cast<int>(firstVisualIndex), 0);
int lastIndex = math_min(static_cast<int>(lastVisualIndex), dataSize);
glColor4f(m_signalColor_r, m_signalColor_g, m_signalColor_b, 0.8);
for (int visualIndex = firstIndex;
visualIndex < lastIndex;
visualIndex += 2) {
GLfloat maxAll = math_max(
data[visualIndex].filtered.all,
data[visualIndex+1].filtered.all);
glVertex2f(float(visualIndex), 0.f);
glVertex2f(float(visualIndex), maxAll);
}
}
glEnd();
}
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
painter->endNativePainting();
}
void JointPendulumController::update()
{
if (!joint_state_->calibrated_)
return;
assert(robot_ != NULL);
double error(0);
ros::Time time = robot_->getTime();
assert(joint_state_->joint_);
dt_= time - last_time_;
if (!initialized_)
{
initialized_ = true;
command_ = joint_state_->position_;
last_position_ = joint_state_->position_;
last_last_position_ = joint_state_->position_;
}
if(joint_state_->joint_->type == urdf::Joint::REVOLUTE)
{
error = joint_state_->position_ - command_;
}
else if(joint_state_->joint_->type == urdf::Joint::CONTINUOUS)
{
error = angles::shortest_angular_distance(command_, joint_state_->position_);
}
else //prismatic
{
error = joint_state_->position_ - command_;
}
double commanded_effort = pid_controller_.updatePid(error, dt_);
//joint_state_->commanded_effort_ = commanded_effort;
// hack for compensation
double g = 1.0;
double p = angles::shortest_angular_distance(joint_state_->position_,M_PI/2.0);
//double f = g*sin(p);
//joint_state_->commanded_effort_ = f;
//ROS_ERROR("debug %f %f %f",g,p,f);
// analytical solution of the pendulum
// d^2 theta / dt^2 + m*g*l / I sin (theta) = 0
// theta_new = 2*theta_1 - theta_2 - dt^2 * m*g*l / I * sin(theta_1)
// we can check error per time step.
// taking last position
double m = 1;
double l = 1;
double I = m*l*l;
bool nj = 1000; // discretize time step into small steps
ros::Time tau = time; // start time
double dtau = dt_.toSec() / (double)nj; // in seconds
double theta = last_position_;
// integrate from last position to this position
for (int j=0;j<nj;j++)
{
// solve for new position
theta = theta + dtau * ( (last_position_ - last_last_position_) / dt_.toSec()
- 0.5 * (dtau + dt_.toSec()) *m * g * l / I * sin(theta) ) ;
}
ROS_WARN("pendulum %20.15f %20.15f",joint_state_->position_,joint_state_->position_ - theta);
// state publishes
if(loop_count_ % 10 == 0) // hard code to publish once every 10 steps
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command_;
controller_state_publisher_->msg_.process_value = joint_state_->position_;
controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
controller_state_publisher_->msg_.error = error;
controller_state_publisher_->msg_.time_step = dt_.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
dummy);
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
last_time_ = time;
last_last_position_ = last_position_;
last_position_ = joint_state_->position_;
}