void FilteredImage::applyFilter(AbstractImageFilter *filter)
{
if(m_filter != 0)
{
disconnect(m_filter,0,this,0);
}
m_filter = filter;
if(m_filter != 0)
{
connect(m_filter,SIGNAL(filterApplied(QImage)),this,SLOT(filterApplied(QImage)));
if(m_filter->parameterList().isEmpty())
{
if(m_filter->applyFilter(m_image))
{
setIsApplyingFilter(true);
}
}
else
{
resetFilter();
}
}
else
{
resetFilter();
}
}
void StandaloneFilterWindow::buttonClicked (Button*)
{
if (filter == 0)
return;
PopupMenu m;
m.addItem (1, TRANS("Audio Settings..."));
m.addSeparator();
m.addItem (2, TRANS("Save current state..."));
m.addItem (3, TRANS("Load a saved state..."));
m.addSeparator();
m.addItem (4, TRANS("Reset to default state"));
switch (m.showAt (optionsButton))
{
case 1:
showAudioSettingsDialog();
break;
case 2:
saveState();
break;
case 3:
loadState();
break;
case 4:
resetFilter();
break;
default:
break;
}
}
void ptToolBox::createMenuActions() {
if (FBlockAction) return; // actions already exist.
FBlockAction = new QAction(this);
connect(FBlockAction, SIGNAL(triggered()), this, SLOT(toggleBlocked()));
FBlockAction->setIcon(QIcon(*Theme->ptIconCircleRed));
FBlockAction->setIconVisibleInMenu(true);
FResetAction = new QAction(QIcon(*Theme->ptIconReset), tr("&Reset"), this);
connect(FResetAction, SIGNAL(triggered()), this, SLOT(resetFilter()));
FResetAction->setIconVisibleInMenu(true);
FSaveAction = new QAction(QIcon(*Theme->ptIconSavePreset), tr("&Save preset"), this);
connect(FSaveAction, SIGNAL(triggered()), this, SLOT(savePreset()));
FSaveAction->setIconVisibleInMenu(true);
FAppendAction = new QAction(QIcon(*Theme->ptIconAppendPreset), tr("&Append preset"), this);
connect(FAppendAction, SIGNAL(triggered()), this, SLOT(appendPreset()));
FAppendAction->setIconVisibleInMenu(true);
FFavAction = new QAction(this);
connect(FFavAction, SIGNAL(triggered()), this, SLOT(toggleFavourite()));
FFavAction->setIconVisibleInMenu(true);
FHideAction = new QAction(QIcon(*Theme->ptIconCrossRed), tr("&Hide"), this);
connect(FHideAction, SIGNAL(triggered()), this, SLOT(toggleHidden()));
FHideAction->setIconVisibleInMenu(true);
}
GUI::FilterConfigurationBox::FilterConfigurationBox(QWidget* parent):QFrame(parent) {
setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Minimum);
createUi();
connect(button_apply_,SIGNAL(clicked()),this,SLOT(applyFilter()));
connect(button_reset_,SIGNAL(clicked()),this,SLOT(resetFilter()));
}
AckermannKalmanFilter::AckermannKalmanFilter(const GrandParent::Parameters&
kfParameters, const Parent::Parameters& diffKfParameters, const
Parameters& ackKfParameters) :
Parent(kfParameters, diffKfParameters),
ackermannKalmanFilterParameters_(ackKfParameters) {
resetFilter();
}
TEST (InterfacesTest, ImuAccBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input2", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
imu.header.frame_id = "base_link";
imu.linear_acceleration_covariance[0] = 1e-6;
imu.linear_acceleration_covariance[4] = 1e-6;
imu.linear_acceleration_covariance[8] = 1e-6;
imu.linear_acceleration.x = 1;
imu.linear_acceleration.y = -1;
imu.linear_acceleration.z = 1;
// Move with constant acceleration for 1 second,
// then check our velocity.
ros::Rate loopRate(50);
for(size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.4);
EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.4);
EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.4);
imu.linear_acceleration.x = 0.0;
imu.linear_acceleration.y = 0.0;
imu.linear_acceleration.z = 0.0;
// Now move for another second, and see where we
// end up
loopRate = ros::Rate(50);
for(size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.2), 0.4);
resetFilter();
}
/**
* set_joints_known_pos()
*
* Set all the mechanism joints to known reference angles.
* Propogate the joint angle to motor position and encoder offset.
*/
int set_joints_known_pos(struct mechanism* _mech, int tool_only)
{
struct DOF* _joint=NULL;
int j=0;
/// Set joint position reference for just tools, or all DOFS
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
if ( tool_only && ! is_toolDOF( _joint->type)) {
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].home_position;
} else if (!tool_only && is_toolDOF(_joint->type) ) {
_joint->jpos_d = _joint->jpos;
} else {
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].max_position;
// Initialize a trajectory to operating angle
_joint->state = jstate_homing1;
}
}
/// Inverse cable coupling: jpos_d ---> mpos_d
invMechCableCoupling(_mech, 1);
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// Reset the state-estimate filter
_joint->mpos = _joint->mpos_d;
resetFilter( _joint );
// Convert the motor position to an encoder offset.
// mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k
float f_enc_val = _joint->enc_val;
// Encoder values on Gold arm are reversed. See also state_machine.cpp
if ( _mech->type == GOLD_ARM)
f_enc_val *= -1.0;
/// Set the joint offset in encoder space.
float cc = ENC_CNTS_PER_REV / (2*M_PI);
_joint->enc_offset = f_enc_val - (_joint->mpos_d * cc);
getStateLPF(_joint);
}
fwdMechCableCoupling(_mech);
return 0;
}
void addToFilter(int pD){
currentMedianSample++;
for (int a = 0 ; a < MAX_MEDIAN_SAMPLES; a++){
if (medianBuffer[a] < pD){
for (int b = MAX_MEDIAN_SAMPLES - 1; b > a; b--){
medianBuffer[b] = medianBuffer[b - 1];
}
medianBuffer[a] = pD;
break;
}
}
if (currentMedianSample == MAX_MEDIAN_SAMPLES){
medianDistance = medianBuffer[int(MAX_MEDIAN_SAMPLES / 2.0)];
resetFilter();
}
}
TEST (InterfacesTest, ImuPoseBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
tf2::Quaternion quat;
quat.setRPY(M_PI/4, -M_PI/4, M_PI/2);
imu.orientation = tf2::toMsg(quat);
for(size_t ind = 0; ind < 9; ind+=4)
{
imu.orientation_covariance[ind] = 1e-6;
}
imu.header.frame_id = "base_link";
// Make sure the pose reset worked. Test will timeout
// if this fails.
ros::Rate loopRate(50);
for(size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
double r, p, y;
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r - M_PI/4), 0.1);
EXPECT_LT(::fabs(p + M_PI/4), 0.1);
EXPECT_LT(::fabs(y - M_PI/2), 0.1);
EXPECT_LT(filtered_.pose.covariance[21], 0.5);
EXPECT_LT(filtered_.pose.covariance[28], 0.25);
EXPECT_LT(filtered_.pose.covariance[35], 0.5);
resetFilter();
}
TEST(InterfacesTest, PoseBasicIO)
{
ros::NodeHandle nh;
ros::Publisher posePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
geometry_msgs::PoseWithCovarianceStamped pose;
pose.pose.pose.position.x = 20.0;
pose.pose.pose.position.y = 10.0;
pose.pose.pose.position.z = -40.0;
pose.pose.pose.orientation.x = 0;
pose.pose.pose.orientation.y = 0;
pose.pose.pose.orientation.z = 0;
pose.pose.pose.orientation.w = 1;
for (size_t ind = 0; ind < 36; ind+=7)
{
pose.pose.covariance[ind] = 1e-6;
}
pose.header.frame_id = "odom";
ros::Rate loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
pose.header.stamp = ros::Time::now();
posePub.publish(pose);
ros::spinOnce();
loopRate.sleep();
pose.header.seq++;
}
// Now check the values from the callback
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - pose.pose.pose.position.x), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.1); // Configuration for this variable for this sensor is false
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - pose.pose.pose.position.z), 0.1);
EXPECT_LT(filtered_.pose.covariance[0], 0.5);
EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false
EXPECT_LT(filtered_.pose.covariance[14], 0.5);
resetFilter();
}
TEST(InterfacesTest, OdomPoseBasicIO)
{
stateUpdated_ = false;
ros::NodeHandle nh;
ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
nav_msgs::Odometry odom;
odom.pose.pose.position.x = 20.0;
odom.pose.pose.position.y = 10.0;
odom.pose.pose.position.z = -40.0;
odom.pose.covariance[0] = 2.0;
odom.pose.covariance[7] = 2.0;
odom.pose.covariance[14] = 2.0;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
// Now check the values from the callback
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); // Configuration for this variable for this sensor is false
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), 0.01);
EXPECT_LT(filtered_.pose.covariance[0], 0.5);
EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false
EXPECT_LT(filtered_.pose.covariance[14], 0.6);
resetFilter();
}
TEST(InterfacesTest, ImuDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher imu0Pub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
ros::Publisher imu1Pub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input3", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
imu.header.frame_id = "base_link";
tf2::Quaternion quat;
const double roll = M_PI/2.0;
const double pitch = -M_PI;
const double yaw = -M_PI/4.0;
quat.setRPY(roll, pitch, yaw);
imu.orientation = tf2::toMsg(quat);
imu.orientation_covariance[0] = 0.02;
imu.orientation_covariance[4] = 0.02;
imu.orientation_covariance[8] = 0.02;
imu.angular_velocity_covariance[0] = 0.02;
imu.angular_velocity_covariance[4] = 0.02;
imu.angular_velocity_covariance[8] = 0.02;
size_t setCount = 0;
while (setCount++ < 10)
{
imu.header.stamp = ros::Time::now();
imu0Pub.publish(imu); // Use this to move the absolute orientation
imu1Pub.publish(imu); // Use this to keep velocities at 0
ros::spinOnce();
ros::Duration(0.1).sleep();
imu.header.seq++;
}
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
ros::Duration(0.1).sleep();
imu.header.seq++;
}
double rollFinal = roll;
double pitchFinal = pitch;
double yawFinal = yaw;
// Move the orientation slowly, and see if we
// can push it to 0
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
yawFinal -= 0.01 * (3.0 * M_PI/4.0);
quat.setRPY(rollFinal, pitchFinal, yawFinal);
imu.orientation = tf2::toMsg(quat);
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
ros::spinOnce();
// Move the orientation slowly, and see if we
// can push it to 0
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
rollFinal += 0.01 * (M_PI/2.0);
quat.setRPY(rollFinal, pitchFinal, yawFinal);
imu.orientation = tf2::toMsg(quat);
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
ros::spinOnce();
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
mat.getRPY(rollFinal, pitchFinal, yawFinal);
EXPECT_LT(::fabs(rollFinal), 0.2);
EXPECT_LT(::fabs(pitchFinal), 0.2);
EXPECT_LT(::fabs(yawFinal), 0.2);
resetFilter();
}
TEST(InterfacesTest, PoseDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher posePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
geometry_msgs::PoseWithCovarianceStamped pose;
pose.pose.pose.position.x = 20.0;
pose.pose.pose.position.y = 10.0;
pose.pose.pose.position.z = -40.0;
pose.pose.pose.orientation.w = 1;
pose.pose.covariance[0] = 2.0;
pose.pose.covariance[7] = 2.0;
pose.pose.covariance[14] = 2.0;
pose.pose.covariance[21] = 0.2;
pose.pose.covariance[28] = 0.2;
pose.pose.covariance[35] = 0.2;
pose.header.frame_id = "odom";
// No guaranteeing that the zero state
// we're expecting to see here isn't just
// a result of zeroing it out previously,
// so check 10 times in succession.
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
pose.header.stamp = ros::Time::now();
posePub.publish(pose);
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
ros::Duration(0.1).sleep();
pose.header.seq++;
}
// ...but only if we give the measurement a tiny covariance
for (size_t ind = 0; ind < 36; ind+=7)
{
pose.pose.covariance[ind] = 1e-6;
}
// Issue this location repeatedly, and see if we get
// a final reported position of (1, 2, -3)
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
pose.pose.pose.position.x += 0.01;
pose.pose.pose.position.y += 0.02;
pose.pose.pose.position.z -= 0.03;
pose.header.stamp = ros::Time::now();
posePub.publish(pose);
ros::spinOnce();
loopRate.sleep();
pose.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
resetFilter();
}
TEST(InterfacesTest, OdomDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
nav_msgs::Odometry odom;
odom.pose.pose.position.x = 20.0;
odom.pose.pose.position.y = 10.0;
odom.pose.pose.position.z = -40.0;
odom.pose.pose.orientation.w = 1;
odom.pose.covariance[0] = 2.0;
odom.pose.covariance[7] = 2.0;
odom.pose.covariance[14] = 2.0;
odom.pose.covariance[21] = 0.2;
odom.pose.covariance[28] = 0.2;
odom.pose.covariance[35] = 0.2;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
// No guaranteeing that the zero state
// we're expecting to see here isn't just
// a result of zeroing it out previously,
// so check 10 times in succession.
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
ros::Duration(0.1).sleep();
odom.header.seq++;
}
for (size_t ind = 0; ind < 36; ind+=7)
{
odom.pose.covariance[ind] = 1e-6;
}
// Slowly move the position, and hope that the
// differential position keeps up
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.pose.pose.position.x += 0.01;
odom.pose.pose.position.y += 0.02;
odom.pose.pose.position.z -= 0.03;
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
resetFilter();
}
TEST(InterfacesTest, ImuTwistBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
tf2::Quaternion quat;
imu.angular_velocity.x = (M_PI / 2.0);
for (size_t ind = 0; ind < 9; ind+=4)
{
imu.angular_velocity_covariance[ind] = 1e-6;
}
imu.header.frame_id = "base_link";
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
double r, p, y;
mat.getRPY(r, p, y);
// Tolerances may seem loose, but the initial state covariances
// are tiny, so the filter is sluggish to accept velocity data
EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7);
EXPECT_LT(::fabs(p), 0.1);
EXPECT_LT(::fabs(y), 0.1);
EXPECT_LT(filtered_.twist.covariance[21], 1e-3);
EXPECT_LT(filtered_.pose.covariance[21], 0.1);
resetFilter();
imu.angular_velocity.x = 0.0;
imu.angular_velocity.y = -(M_PI / 2.0);
// Make sure the pose reset worked. Test will timeout
// if this fails.
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 0.1);
EXPECT_LT(::fabs(p + M_PI / 2.0), 0.7);
EXPECT_LT(::fabs(y), 0.1);
EXPECT_LT(filtered_.twist.covariance[28], 1e-3);
EXPECT_LT(filtered_.pose.covariance[28], 0.1);
resetFilter();
imu.angular_velocity.y = 0;
imu.angular_velocity.z = M_PI / 4.0;
// Make sure the pose reset worked. Test will timeout
// if this fails.
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 0.1);
EXPECT_LT(::fabs(p), 0.1);
EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7);
EXPECT_LT(filtered_.twist.covariance[35], 1e-3);
EXPECT_LT(filtered_.pose.covariance[35], 0.1);
resetFilter();
// Test to see if the angular velocity data is ignored when we set the
// first covariance value to -1
sensor_msgs::Imu imuIgnore;
imuIgnore.angular_velocity.x = 100;
imuIgnore.angular_velocity.y = 100;
imuIgnore.angular_velocity.z = 100;
imuIgnore.angular_velocity_covariance[0] = -1;
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imuIgnore.header.stamp = ros::Time::now();
imuPub.publish(imuIgnore);
loopRate.sleep();
ros::spinOnce();
imuIgnore.header.seq++;
}
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 1e-3);
EXPECT_LT(::fabs(p), 1e-3);
EXPECT_LT(::fabs(y), 1e-3);
resetFilter();
}
TEST(InterfacesTest, TwistBasicIO)
{
ros::NodeHandle nh;
ros::Publisher twistPub = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("/twist_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
geometry_msgs::TwistWithCovarianceStamped twist;
twist.twist.twist.linear.x = 5.0;
twist.twist.twist.linear.y = 0;
twist.twist.twist.linear.z = 0;
twist.twist.twist.angular.x = 0;
twist.twist.twist.angular.y = 0;
twist.twist.twist.angular.z = 0;
for (size_t ind = 0; ind < 36; ind+=7)
{
twist.twist.covariance[ind] = 1e-6;
}
twist.header.frame_id = "base_link";
ros::Rate loopRate = ros::Rate(20);
for (size_t i = 0; i < 400; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0);
resetFilter();
twist.twist.twist.linear.x = 0.0;
twist.twist.twist.linear.y = 5.0;
loopRate = ros::Rate(20);
for (size_t i = 0; i < 200; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0);
resetFilter();
twist.twist.twist.linear.y = 0.0;
twist.twist.twist.linear.z = 5.0;
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0);
resetFilter();
twist.twist.twist.linear.z = 0.0;
twist.twist.twist.linear.x = 1.0;
twist.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05);
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5);
resetFilter();
twist.twist.twist.linear.x = 0.0;
twist.twist.twist.angular.z = 0.0;
twist.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05);
// First, roll the vehicle on its side
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
twist.twist.twist.angular.x = 0.0;
twist.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05);
// Now, pitch it down (positive pitch velocity in vehicle frame)
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
twist.twist.twist.angular.y = 0.0;
twist.twist.twist.linear.x = 3.0;
// We should now be on our side and facing -Y. Move forward in
// the vehicle frame X direction, and make sure Y decreases in
// the world frame.
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0);
resetFilter();
}
// POST set params as "fanmode=1"
void HVAC::setVar(String sCmd, int val)
{
if(m_EE.bLock) return;
switch( CmdIdx( sCmd, cSCmds ) )
{
case 0: // fanmode
if(val == 2) // "freshen"
{
if(m_bRunning || m_furnaceFan || m_bFanMode) // don't run if system or fan is running
break;
m_fanPostTimer = m_EE.fanCycleTime; // use the post fan timer to shut off
fanSwitch(true);
}
else setFan( (val) ? true:false);
break;
case 1: // mode
setMode( val );
break;
case 2: // heatmode
setHeatMode( val );
break;
case 3: // resettotal
resetTotal();
break;
case 4:
resetFilter();
break;
case 5: // fanpostdelay
m_EE.fanPostDelay[digitalRead(P_REV)] = constrain(val, 0, 60*5); // Limit 0 to 5 minutes
break;
case 6: // cyclemin
m_EE.cycleMin = constrain(val, 60, 60*20); // Limit 1 to 20 minutes
break;
case 7: // cyclemax
m_EE.cycleMax = constrain(val, 60*2, 60*60); // Limit 2 to 60 minutes
break;
case 8: // idlemin
m_EE.idleMin = constrain(val, 60, 60*30); // Limit 1 to 30 minutes
break;
case 9: // cyclethresh
m_EE.cycleThresh = constrain(val, 5, 50); // Limit 0.5 to 5.0 degrees
break;
case 10: // cooltempl
setTemp(Mode_Cool, val, 0);
m_bRecheck = true; // faster update
break;
case 11: // cooltemph
setTemp(Mode_Cool, val, 1);
m_bRecheck = true;
break;
case 12: // heattempl
setTemp(Mode_Heat, val, 0);
m_bRecheck = true;
break;
case 13: // heattemph
setTemp(Mode_Heat, val, 1);
m_bRecheck = true;
break;
case 14: // eheatthresh
m_EE.eHeatThresh = constrain(val, 5, 50); // Limit 5 to 50 degrees F
break;
case 15: // override
if(val == 0) // cancel
{
m_ovrTemp = 0;
m_overrideTimer = 0;
}
else
{
m_ovrTemp = constrain(val, -90, 90); // Limit to -9.0 to +9.0 degrees F
m_overrideTimer = m_EE.overrideTime;
}
m_bRecheck = true;
break;
case 16: // overridetime
m_EE.overrideTime = constrain(val, 60*1, 60*60*6); // Limit 1 min to 6 hours
break;
case 17: // humidmode
m_EE.humidMode = constrain(val, HM_Off, HM_Auto2);
break;
case 18: // humidl
m_EE.rhLevel[0] = constrain(val, 300, 900); // no idea really
break;
case 19: // humidh
m_EE.rhLevel[1] = constrain(val, 300, 900);
break;
case 20: // adj
m_EE.adj = constrain(val, -30, 30); // calibrate can only be +/-3.0
break;
case 21: // fanPretime
m_EE.fanPreTime[m_EE.Mode == Mode_Heat] = constrain(val, 0, 60*5); // Limit 0 to 5 minutes
break;
case 22: // fancycletime
m_EE.fanCycleTime = val;
break;
case 23: // rmtflgs 0xC=(RF_RL|RF_RH) = use remote, 0x3=(RFML|RF_MH)= use main, 0xF = use both averaged
m_RemoteFlags = val;
break;
case 24: // awaytime
m_EE.awayTime = val; // no limit
break;
case 25: // awaydelta
if(m_EE.Mode == Mode_Heat)
m_EE.awayDelta[1] = constrain(val, -150, 0); // Limit to -15 degrees (heat away) target is constrained in calcTargetTemp
else
m_EE.awayDelta[0] = constrain(val, 0, 150); // Limit +15 degrees (cool away)
break;
case 26: // away (uses the override feature)
if(val) // away
{
m_overrideTimer = m_EE.awayTime * 60; // convert minutes to seconds
m_ovrTemp = m_EE.awayDelta[m_EE.Mode == Mode_Heat];
m_bAway = true;
}
else // back
{
m_ovrTemp = 0;
m_overrideTimer = 0;
m_bAway = false;
}
break;
}
}
/**
* set_joints_known_pos()
*
* \param _mech which mechanism (gold/green)
* \param tool_only flag which initializes only the tool/wrist joints
*
* \brief Set joint angles to known values after hard stops are reached.
*
* Set all the mechanism joints to known reference angles.
* Propagate the joint angle to motor position and encoder offset.
*
* \todo Rationalize the sign changes on GREEN_ARM vs GOLD_ARM (see IFDEF below).
* \todo This MAYBE needs to be changed to support device specific parameter changes read from a config file or ROS service.
* \ingroup Control
*/
int set_joints_known_pos(struct mechanism* _mech, int tool_only)
{
struct DOF* _joint=NULL;
int j=0;
// int offset = 0;
// if (_mech->type == GREEN_ARM) offset = 8;
/// Set joint position reference for just tools, or all DOFS
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// when tool joints finish, set positioning joints to neutral
if ( tool_only && ! is_toolDOF( _joint->type))
{
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].home_position; // keep non tool joints from moving
}
// when positioning joints finish, set tool joints to nothing special
else if (!tool_only && is_toolDOF(_joint->type) )
{
_joint->jpos_d = _joint->jpos;
}
// when tool or positioning joints finish, set them to max_angle
else
{
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].max_position;
// Initialize a trajectory to operating angle
_joint->state = jstate_homing1;
}
}
/// Inverse cable coupling: jpos_d ---> mpos_d
// use_actual flag triggered for insertion axis
invMechCableCoupling(_mech, 1);
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// Reset the state-estimate filter
_joint->mpos = _joint->mpos_d;
resetFilter( _joint );
// Convert the motor position to an encoder offset.
// mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k
float f_enc_val = _joint->enc_val;
// Encoder values on Gold arm are reversed. See also state_machine.cpp
switch (_mech->tool_type)
{
case dv_adapter:
if ( _mech->type == GOLD_ARM && !is_toolDOF(_joint))
f_enc_val *= -1.0;
break;
case RII_square_type:
//Green arm tools are also reversed with square pattern
if (( _mech->type == GOLD_ARM && !is_toolDOF(_joint) ) ||
( _mech->type == GREEN_ARM && is_toolDOF(_joint) ))
f_enc_val *= -1.0;
break;
default:
if ( _mech->type == GOLD_ARM || is_toolDOF(_joint) )
f_enc_val *= -1.0;
break;
//Needs a true default/error state
}
/// Set the joint offset in encoder space.
float cc = ENC_CNTS_PER_REV / (2*M_PI);
_joint->enc_offset = f_enc_val - (_joint->mpos_d * cc);
getStateLPF(_joint, _mech->tool_type);
}
fwdMechCableCoupling(_mech);
return 0;
}
Filter::Filter()
{
pthread_mutex_init( &mutex, NULL );
resetFilter();
}
void pingSetup(){
pinMode(TRIGGER, OUTPUT);
pinMode(ECHO, INPUT);
if (maxTimeNeeded < minimumDelay) maxTimeNeeded = minimumDelay;
resetFilter();
}
/**
* \fn int set_joints_known_pos(struct mechanism* _mech, int tool_only)
*
* \brief Set joint angles to known values after hard stops are reached.
*
* \desc Set all the mechanism joints to known reference angles.
* Propagate the joint angle to motor position and encoder offset.
*
* \param _mech which mechanism (gold/green)
* \param tool_only flag which initializes only the tool/wrist joints
*
* \ingroup Control
*
* \return 0
*
* \todo Rationalize the sign changes on GREEN_ARM vs GOLD_ARM (see IFDEF below).
* \todo This MAYBE needs to be changed to support device specific parameter changes read from a config file or ROS service.
*/
int set_joints_known_pos(struct mechanism* _mech, int tool_only)
{
struct DOF* _joint=NULL;
int j=0;
int scissor = ((_mech->mech_tool.t_end == mopocu_scissor) || (_mech->mech_tool.t_end == potts_scissor))? 1 : 0;
// int offset = 0;
// if (_mech->type == GREEN_ARM) offset = 8;
/// Set joint position reference for just tools, or all DOFS
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// when tool joints finish, set positioning joints to neutral
if ( tool_only && ! is_toolDOF( _joint->type))
{
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].home_position; // keep non tool joints from moving
}
// when positioning joints finish, set tool joints to nothing special
else if (!tool_only && is_toolDOF(_joint->type) )
{
_joint->jpos_d = _joint->jpos;
}
// when tool or positioning joints finish, set them to max_angle
else {
// Set jpos_d to the joint limit value.
if (scissor && ((_joint->type == GRASP2_GREEN) || (_joint->type == GRASP2_GOLD))){
_joint->jpos_d = _mech->mech_tool.grasp2_min_angle;
log_msg("setting grasp 2 to arm %d, joint %d to value %f", _mech->mech_tool.mech_type, _joint->type, _joint->jpos_d);
}
else
_joint->jpos_d = DOF_types[_joint->type].max_position;
// Initialize a trajectory to operating angle
_joint->state = jstate_homing1;
}
}
/// Inverse cable coupling: jpos_d ---> mpos_d
// use_actual flag triggered for insertion axis
invMechCableCoupling(_mech, 1);
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// Reset the state-estimate filter
_joint->mpos = _joint->mpos_d;
resetFilter( _joint );
// Convert the motor position to an encoder offset.
// mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k
float f_enc_val = _joint->enc_val;
// Encoder values on Gold arm are reversed. See also state_machine.cpp
switch (_mech->mech_tool.t_style){
case dv:
if ( _mech->type == GOLD_ARM && !is_toolDOF(_joint))
f_enc_val *= -1.0;
break;
case square_raven:
if ( ( _mech->type == GOLD_ARM && !is_toolDOF(_joint) )
||
( _mech->type == GREEN_ARM && is_toolDOF(_joint) ) //Green arm tools are also reversed with square pattern
)
f_enc_val *= -1.0;
break;
default:
if ( _mech->type == GOLD_ARM || is_toolDOF(_joint) )
f_enc_val *= -1.0;
break;
}
#ifdef OPPOSE_GRIP
if (j == GRASP1){
f_enc_val *= -1;// switch encoder value for opposed grasp
// static int twice = 0;
// if (twice < 2){
// log_msg("homing enc_val swapped");
// twice++;
}
#endif
/// Set the joint offset in encoder space.
float cc = ENC_CNTS_PER_REV / (2*M_PI);
_joint->enc_offset = f_enc_val - (_joint->mpos_d * cc);
getStateLPF(_joint, _mech->tool_type);
//getStateLPF(_joint, _mech->mech_tool.t_style);
}
fwdMechCableCoupling(_mech);
return 0;
}
CKF::CKF(){
resetFilter();
}
QWidget *Desktopwidget::createToolbar(void)
{
QWidget *group = new QWidget (this);
//TODO: Move this to use the designer
/* create the desktop toolbar. We are doing this manually since we can't
seem to get Qt to insert a QLineEdit into a toolbar */
_toolbar = new QToolBar (group);
// _toolbar = new QWidget (group);
// _toolbar = group;
addAction (_actionPprev, "Previous page", SLOT(pageLeft ()), "", _toolbar, "pprev.xpm");
addAction (_actionPprev, "Next page", SLOT(pageRight ()), "", _toolbar, "pnext.xpm");
addAction (_actionPprev, "Previous stack", SLOT(stackLeft ()), "", _toolbar, "prev.xpm");
addAction (_actionPprev, "Next stack", SLOT(stackRight ()), "", _toolbar, "next.xpm");
QWidget *findgroup = new QWidget (_toolbar);
QHBoxLayout *hboxLayout2 = new QHBoxLayout();
hboxLayout2->setSpacing(0);
hboxLayout2->setContentsMargins (0, 0, 0, 0);
hboxLayout2->setObjectName(QString::fromUtf8("hboxLayout2"));
findgroup->setLayout (hboxLayout2);
QLabel *label = new QLabel (findgroup);
label->setText(QApplication::translate("Mainwindow", "Filter:", 0));
label->setObjectName(QString::fromUtf8("label"));
hboxLayout2->addWidget(label);
_match = new QLineEdit (findgroup);
_match->setObjectName ("match");
QSizePolicy sizePolicy2(QSizePolicy::Expanding, QSizePolicy::Fixed);
sizePolicy2.setHorizontalStretch(1);
sizePolicy2.setVerticalStretch(0);
sizePolicy2.setHeightForWidth(_match->sizePolicy().hasHeightForWidth());
_match->setSizePolicy(sizePolicy2);
_match->setMinimumSize(QSize(50, 0));
//_match->setDragEnabled(true);
connect (_match, SIGNAL (returnPressed()),
this, SLOT (matchUpdate ()));
connect (_match, SIGNAL (textChanged(const QString&)),
this, SLOT (matchChange (const QString &)));
//_reset_filter = new QAction (this);
//_reset_filter->setShortcut (Qt::Key_Escape);
//connect (_reset_filter, SIGNAL (triggered()), this, SLOT (resetFilter()));
//_match->addAction (_reset_filter);
//_match->installEventFilter (this);
// When ESC is pressed, clear the field
QStateMachine *machine = new QStateMachine (this);
QState *s1 = new QState (machine);
// QSignalTransition *pressed_esc = new QSignalTransition(_match,
// SIGNAL(textChanged(const QString&)));
QKeyEventTransition *pressed_esc = new QKeyEventTransition(_match,
QEvent::KeyPress, Qt::Key_Escape);
s1->addTransition (pressed_esc);
connect(pressed_esc, SIGNAL(triggered()), this, SLOT(resetFilter()));
machine->setInitialState (s1);
machine->start ();
#if 0
QPushButton *test = new QPushButton (findgroup);
test->setText ("hello");
hboxLayout2->addWidget (test);
QStateMachine *test_machine = new QStateMachine (this);
QState *test_s1 = new QState (test_machine);
QSignalTransition *trans = new QSignalTransition(test, SIGNAL(clicked()));
test_s1->addTransition (trans);
connect(trans, SIGNAL(triggered()), this, SLOT(resetFilter()));
test_machine->setInitialState (test_s1);
test_machine->start ();
#endif
// and change the state
hboxLayout2->addWidget(label);
hboxLayout2->addWidget (_match);
addAction (_find, "Filter stacks", SLOT(findClicked ()), "", findgroup, "find.xpm");
QToolButton *find = new QToolButton (findgroup);
find->setDefaultAction (_find);
hboxLayout2->addWidget (find);
// connect (_find, SIGNAL (activated ()), this, SLOT (findClicked ()));
QSpacerItem *spacerItem = new QSpacerItem(16, 20, QSizePolicy::Expanding, QSizePolicy::Minimum);
hboxLayout2->addItem (spacerItem);
_global = new QCheckBox("Subdirs", findgroup);
_global->setObjectName(QString::fromUtf8("global"));
hboxLayout2->addWidget(_global);
addAction (_reset, "Reset", SLOT(resetFilter ()), "", findgroup);
QToolButton *reset = new QToolButton (findgroup);
reset->setDefaultAction (_reset);
hboxLayout2->addWidget (reset);
_toolbar->addWidget (findgroup);
#ifndef QT_NO_TOOLTIP
_match->setToolTip(QApplication::translate("Mainwindow", "Enter part of the name of the stack to search for", 0));
_find->setToolTip(QApplication::translate("Mainwindow", "Search for the name", 0));
_global->setToolTip(QApplication::translate("Mainwindow", "Enable this to search all subdirectories also", 0));
_reset->setToolTip(QApplication::translate("Mainwindow", "Reset the search string", 0));
#endif // QT_NO_TOOLTIP
#ifndef QT_NO_WHATSTHIS
_match->setWhatsThis(QApplication::translate("Mainwindow", "The filter feature can be used in two ways. To filter out unwanted stacks, type a few characters from the stack name that you are looking for. Everything that does not match will be removed from view. To go back, just delete characters from the filter.\n"
"\n"
"There is also a 'global' mode which allows searching of all subdirectories. To use this, select the 'global' button, then type your filter string. Press return or click 'find' to perform the search. This might take a while.\n"
"\n"
"To reset the filter, click the 'reset' button.", 0));
_find->setWhatsThis(QApplication::translate("Mainwindow", "Click this button to perform a search when in global mode", 0));
_global->setWhatsThis(QApplication::translate("Mainwindow", "The filter feature can be used in two ways. To filter out unwanted stacks, type a few characters from the stack name that you are looking for. Everything that does not match will be removed from view. To go back, just delete characters from the filter.\n"
"\n"
"There is also a 'global' mode which allows searching of all subdirectories. To use this, select the 'global' button, then type your filter string. Press return or click 'find' to perform the search. This might take a while.\n"
"\n"
"To reset the filter, click the 'reset' button.", 0));
_reset->setWhatsThis(QApplication::translate("Mainwindow", "Press this button to reset the filter string and display stacks in the current directory", 0));
#endif // QT_NO_WHATSTHIS
return group;
}
KalmanFilter::KalmanFilter(const Parameters& parameters) :
kalmanFilterParameters_(parameters) {
resetFilter();
}
