SlaveController::SlaveController(ros::NodeHandle & n_,
double freq_,
Eigen::Matrix<double,6,1> Kp_,
Eigen::Matrix<double,6,1> Kd_,
Eigen::Matrix<double,6,1> Bd_,
Eigen::Matrix<double,6,6> lambda_,
Eigen::Matrix<double,6,1> master_to_slave_scale_,
Eigen::Matrix<double,6,1> master_pose_slave_velocity_scale_,
Eigen::Matrix<double,6,1> master_min_,
Eigen::Matrix<double,6,1> master_max_,
Eigen::Matrix<double,6,1> slave_min_,
Eigen::Matrix<double,6,1> slave_max_,
Eigen::Matrix<double,6,1> slave_velocity_min_,
Eigen::Matrix<double,6,1> slave_velocity_max_) :
master_to_slave_scale(master_to_slave_scale_),
master_pose_slave_velocity_scale(master_pose_slave_velocity_scale_),
Controller(n_,freq_, Kp_, Kd_, Bd_, lambda_, master_min_, master_max_, slave_min_, slave_max_, slave_velocity_min_, slave_velocity_max_)
{
initParams();
//slave_callback_type = boost::bind(&SlaveController::paramsCallback, this, _1, _2);
//slave_server.setCallback(slave_callback_type);
// Feedback publish
cmd_pub = n.advertise<geometry_msgs::Twist>("/RosAria/cmd_vel", 1);
// Master joint states subscriber
master_sub = n.subscribe<sensor_msgs::JointState>("/omni1_joint_states", 1, &SlaveController::masterJointsCallback, this);
// Slave pose and velocity subscriber
slave_sub = n.subscribe("/RosAria/pose", 1, &SlaveController::slaveOdometryCallback, this);
}
int main(int argc, char *argv[])
{
param params;
AABB water_volume;
AABB boundary_volume;
fluid_particle *fluid_particles = NULL;
boundary_particle *boundary_particles = NULL;
initParams(&water_volume, &boundary_volume, ¶ms);
initParticles(&fluid_particles, &boundary_particles, &water_volume,
&boundary_volume, ¶ms);
eulerStart(fluid_particles, boundary_particles, ¶ms);
// Main simulation loop
#pragma acc enter data copyin(fluid_particles[0:params.number_fluid_particles], boundary_particles[0:params.number_boundary_particles],params[0:1])
for(int n=0; n<params.number_steps; n++) {
updatePressures(fluid_particles, ¶ms);
updateAccelerations(fluid_particles, boundary_particles, ¶ms);
updatePositions(fluid_particles, ¶ms);
if (n % params.steps_per_frame == 0) {
#pragma acc update host(fluid_particles[0:params.number_fluid_particles])
writeFile(fluid_particles, ¶ms);
}
}
#pragma acc exit data delete(fluid_particles[0:params.number_fluid_particles],boundary_particles[0:params.number_boundary_particles],params[0:1])
finalizeParticles(fluid_particles, boundary_particles);
return 0;
}
int main(int argc, char *argv[])
{
param params;
AABB water_volume;
AABB boundary_volume;
fluid_particle *fluid_particles = NULL;
boundary_particle *boundary_particles = NULL;
initParams(&water_volume, &boundary_volume, ¶ms);
initParticles(&fluid_particles, &boundary_particles, &water_volume,
&boundary_volume, ¶ms);
eulerStart(fluid_particles, boundary_particles, ¶ms);
// Main simulation loop
// Hint: Copy fluid_particles(params.number_fluid_particles), boundary_particles(params.number_boundary_particles), and params(1)
for(int n=0; n<params.number_steps; n++) {
updatePressures(fluid_particles, ¶ms);
updateAccelerations(fluid_particles, boundary_particles, ¶ms);
updatePositions(fluid_particles, ¶ms);
if (n % params.steps_per_frame == 0) {
// Hint: Make sure to update the host fluid_particle data
writeFile(fluid_particles, ¶ms);
}
}
// Hint: delete any acc data allocated
finalizeParticles(fluid_particles, boundary_particles);
return 0;
}
// -----------------------------------------------------------------------------
// CSipServerCore::LoadRequestHandlerL
// -----------------------------------------------------------------------------
//
void CSipServerCore::LoadRequestHandlerL()
{
RImplInfoPtrArray infoArray;
REComSession::ListImplementationsL(KSIPRequestHandlerIFUid,
infoArray);
CleanupResetAndDestroyPushL(infoArray);
if (infoArray.Count() > 0 && infoArray[ 0 ])
{
CImplementationInformation& info = *(infoArray[ 0 ]);
TSIPRequestHandlerInitParams initParams(KServerUid3,
iRoutingRequestStore);
TEComResolverParams resolverParams;
resolverParams.SetDataType(info.DataType());
iRequestHandler =
reinterpret_cast< CSIPRequestHandlerBase* >(
REComSession::CreateImplementationL(
KSIPRequestHandlerIFUid,
_FOFF(CSIPRequestHandlerBase,iInstanceKey),
&initParams,
resolverParams));
}
else
{
User::Leave(KErrNotFound);
}
CleanupStack::PopAndDestroy(1); // infoArray
}
void ViBenchMarker3::nextFile()
{
if(mFiles.isEmpty())
{
quit();
}
else
{
initParams();
for(int i = 0; i < mParamsStart.size(); ++i) mParamsCurrent[i] = mParamsStart[i];
mCurrentFile = mFiles.dequeue();
mBestMatthews = 0;
printFileHeader();
mCurrentObject->clearBuffers();
mCurrentObject.setNull();
mCurrentObject = ViAudioObject::create();
mCurrentObject->setFilePath(ViAudio::Target, mCurrentFile);
QObject::connect(mCurrentObject.data(), SIGNAL(decoded()), this, SLOT(process1()));
mCurrentObject->decode();
}
}
int main(int argc, char *argv[])
{
param params;
AABB water_volume;
AABB boundary_volume;
fluid_particle *fluid_particles = NULL;
boundary_particle *boundary_particles = NULL;
initParams(&water_volume, &boundary_volume, ¶ms);
initParticles(&fluid_particles, &boundary_particles, &water_volume,
&boundary_volume, ¶ms);
eulerStart(fluid_particles, boundary_particles, ¶ms);
// Main simulation loop
for(int n=0; n<params.number_steps; n++) {
updatePressures(fluid_particles, ¶ms);
updateAccelerations(fluid_particles, boundary_particles, ¶ms);
updatePositions(fluid_particles, ¶ms);
if (n % params.steps_per_frame == 0) {
writeFile(fluid_particles, ¶ms);
}
}
finalizeParticles(fluid_particles, boundary_particles);
return 0;
}
btSliderConstraint::btSliderConstraint()
:btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true),
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
{
initParams();
}
btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA)
{
initParams();
} // btSliderConstraint::btSliderConstraint()
EchoFilterInstance::EchoFilterInstance(EchoFilter *aParent)
{
mParent = aParent;
mBuffer = 0;
mBufferLength = 0;
mOffset = 0;
initParams(1);
}
void ImageResizer::onInit() {
raw_width_ = 0;
raw_height_ = 0;
DiagnosticNodelet::onInit();
initParams();
initReconfigure();
initPublishersAndSubscribers();
onInitPostProcess();
}
SparseTrackerAM::SparseTrackerAM(ros::NodeHandle nh, ros::NodeHandle nh_private):
nh_(nh),
nh_private_(nh_private),
frame_count_(0),
initialized_(false)
{
ROS_INFO("Starting Sparse Tracker AM");
srand(time(NULL));
// **** init variables
f2b_.setIdentity();
for (int i = 0; i < 20; ++i)
pf2b_[i].setIdentity();
v_x_ = 0;
v_y_ = 0;
v_z_ = 0;
v_roll_ = 0;
v_pitch_ = 0;
v_roll_ = 0;
v_linear_ = btVector3(0.0, 0.0, 0.0);
v_angular_ = btVector3(0.0, 0.0, 0.0);
prev_time_ = ros::Time::now();
// **** init parameters
initParams();
model_ = boost::make_shared<PointCloudOrb>();
model_->header.frame_id = fixed_frame_;
// **** publishers
orb_features_pub_ = nh_.advertise<PointCloudT>(
pub_orb_features_topic_, 1);
canny_features_pub_ = nh_.advertise<PointCloudT>(
pub_canny_features_topic_, 1);
orb_model_pub_ = nh_.advertise<PointCloudT>(
pub_orb_model_topic_, 1);
canny_model_pub_ = nh_.advertise<PointCloudT>(
pub_canny_model_topic_, 1);
pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(pub_pose_topic_, 1);
marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(
"visualization_markers", 1);
// **** subscribers
point_cloud_subscriber_ = nh_.subscribe<PointCloudT>(
sub_topic_, 1, &SparseTrackerAM::pointCloudCallback, this);
}
// constructor
ParseSerial::ParseSerial():nh(){
// initialize parameters
initParams();
// initialize publishers
initPublishers();
// initialize subscribers
initSubscribers();
}
hky::hky(const MDOUBLE inProb_a,
const MDOUBLE inProb_c,
const MDOUBLE inProb_g,
const MDOUBLE inProb_t,
const MDOUBLE TrTv) {
_freq.resize(4);
_freq[0] = inProb_a; _freq[1] = inProb_c;
_freq[2] = inProb_g; _freq[3] = inProb_t;
initParams(TrTv);
}
LibVision::LibVision() {
#if DEBUG_MODE
LIB_VISION_DPRINT("init");
#endif
// Populate lookup table
this->lbFunctions.insert(
std::make_pair("openCamera", &LibVision::openCamera));
this->lbFunctions.insert(
std::make_pair("closeCamera", &LibVision::closeCamera));
this->lbFunctions.insert(
std::make_pair("acquireFrame", &LibVision::acquireFrame));
this->lbFunctions.insert(std::make_pair("saveFrame", &LibVision::saveFrame));
this->lbFunctions.insert(
std::make_pair("loadImageFromMem", &LibVision::loadImageFromMemory));
this->lbFunctions.insert(
std::make_pair("preprocessingOTSU", &LibVision::preprocessingFrameOTSU));
this->lbFunctions.insert(
std::make_pair("preprocessingADPT", &LibVision::preprocessingFrameADPT));
this->lbFunctions.insert(
std::make_pair("detectSquares", &LibVision::detectSquares));
this->lbFunctions.insert(
std::make_pair("detectCircles", &LibVision::detectCircles));
this->lbFunctions.insert(
std::make_pair("detectPenta", &LibVision::detectPenta));
this->lbFunctions.insert(std::make_pair("detectExa", &LibVision::detectExa));
this->lbFunctions.insert(
std::make_pair("holdSquarePatterns", &LibVision::checkSquarePatterns));
this->lbFunctions.insert(
std::make_pair("holdOnlyRightColored", &LibVision::checkColor));
this->lbFunctions.insert(
std::make_pair("setCurrentBackground", &LibVision::setCurrentBackground));
this->lbFunctions.insert(
std::make_pair("subtractBackground", &LibVision::subtractBackground));
this->lbFunctions.insert(
std::make_pair("saveCandidates", &LibVision::saveCandidates));
this->lbFunctions.insert(
std::make_pair("clearCandidates", &LibVision::clearCandidates));
this->lbFunctions.insert(
std::make_pair("drawCandidates", &LibVision::drawAllCandidates));
// Lookup table debug functions
this->lbFunctions.insert(
std::make_pair("printPolygonsFounds", &LibVision::debugPrintPolys));
this->lbFunctions.insert(
std::make_pair("printMethods", &LibVision::debugPrintMethods));
// Print available functions
#if SHOW_INFO
this->debugPrintMethods();
#endif
// Alloc lbParams
initParams();
}
void FELNumerical::FELsimulation1D()
{
if (disflag)
importDisfile();
else
{
generateDistribution(-PI,PI);
}
initParams();
FELsolverSingleFrequency1D();
}
LofiFilterInstance::LofiFilterInstance(LofiFilter *aParent)
{
mParent = aParent;
initParams(3);
mParam[SAMPLERATE] = aParent->mSampleRate;
mParam[BITDEPTH] = aParent->mBitdepth;
mChannelData[0].mSample = 0;
mChannelData[0].mSamplesToSkip = 0;
mChannelData[1].mSample = 0;
mChannelData[1].mSamplesToSkip = 0;
}
VisualOdometry::VisualOdometry(
const ros::NodeHandle& nh,
const ros::NodeHandle& nh_private):
nh_(nh),
nh_private_(nh_private),
initialized_(false),
frame_count_(0)
{
ROS_INFO("Starting RGBD Visual Odometry");
// **** initialize ROS parameters
initParams();
// **** inititialize state variables
f2b_.setIdentity();
// **** publishers
odom_publisher_ = nh_.advertise<OdomMsg>(
"vo", queue_size_);
pose_stamped_publisher_ = nh_.advertise<geometry_msgs::PoseStamped>(
"pose", queue_size_);
path_pub_ = nh_.advertise<PathMsg>(
"path", queue_size_);
feature_cloud_publisher_ = nh_.advertise<PointCloudFeature>(
"feature/cloud", 1);
feature_cov_publisher_ = nh_.advertise<visualization_msgs::Marker>(
"feature/covariances", 1);
model_cloud_publisher_ = nh_.advertise<PointCloudFeature>(
"model/cloud", 1);
model_cov_publisher_ = nh_.advertise<visualization_msgs::Marker>(
"model/covariances", 1);
reset_pos_ = nh_.advertiseService(
"reset_pos_", &VisualOdometry::resetposSrvCallback, this);
// **** subscribers
ImageTransport rgb_it(nh_);
ImageTransport depth_it(nh_);
sub_rgb_.subscribe(rgb_it, "/camera/rgb/image_rect_color", queue_size_);
sub_depth_.subscribe(depth_it, "/camera/depth_registered/image_rect_raw", queue_size_);
sub_info_.subscribe(nh_, "/camera/rgb/camera_info", queue_size_);
// Synchronize inputs.
sync_.reset(new RGBDSynchronizer3(
RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_));
sync_->registerCallback(boost::bind(&VisualOdometry::RGBDCallback, this, _1, _2, _3));
}
static void *
barcode_init (Display *dpy, Window win)
{
struct state *st = (struct state *) calloc (1, sizeof(*st));
st->dpy = dpy;
st->window = win;
initParams (st);
setup (st);
setupModel (st);
return st;
}
btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
m_useSolveConstraintObsolete(false),
m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA)
{
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
initParams();
}
void UT_CIceHostResolver::SetupL( )
{
iMultiplexer = CNcmConnectionMultiplexer::NewL( *this );
iMultiplexer->iConnectionNotifyEnabled = ETrue;
iSessionId = iMultiplexer->CreateSessionL( KTestIap, 5090, 5500 );
iStreamId = iMultiplexer->CreateStreamL( iSessionId, 0, KProtocolInetUdp );
CNATFWPluginApi::TNATFWPluginInitParams initParams(
*this, *iMultiplexer, KTestDomain(), KTestIap );
iResolver = CIceHostResolver::NewL( &initParams );
}
static struct nerverotstate
* nerverot_init (int w, int h)
{
struct nerverotstate *st = (struct nerverotstate *) calloc (1, sizeof(*st));
initParams (st);
setup (st,w,h);
/* make a valid set to erase at first */
renderSegs (st);
return st;
}
virtual void SetUp()
{
initParams();
pose_start = KDL::Frame(
// KDL::Rotation::Quaternion(-0.394948, 0.779915, 0.163176, 0.457299),
// KDL::Vector(0.526501, -1.0469, 1.07618));
KDL::Rotation(-0.1950, -0.4692, 0.8613, -0.4962, 0.8047, 0.3260, -0.8460, -0.3638, -0.3898),
KDL::Vector(-0.1862, -0.1699, 0.1124));
// KDL::Rotation(-0.6314, 0.3683, 0.0689, 0.1038, 0.0905, 0.0912, 0.9958, 0.1032, 0.9945),
// KDL::Vector(-0.7004, 0.1520, 0.2));
}
////////////////////////////////////////////////////////////////////////////////
// Program main
////////////////////////////////////////////////////////////////////////////////
int
main(int argc, char** argv)
{
numParticles = 1024;
uint gridDim = 64;
numIterations = 1;
cutGetCmdLineArgumenti( argc, (const char**) argv, "n", (int *) &numParticles);
cutGetCmdLineArgumenti( argc, (const char**) argv, "grid", (int *) &gridDim);
gridSize.x = gridSize.y = gridSize.z = gridDim;
printf("grid: %d x %d x %d = %d cells\n", gridSize.x, gridSize.y, gridSize.z, gridSize.x*gridSize.y*gridSize.z);
bool benchmark = !cutCheckCmdLineFlag(argc, (const char**) argv, "noqatest") != 0;
cutGetCmdLineArgumenti( argc, (const char**) argv, "i", &numIterations);
cudaInit(argc, argv);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutInitWindowSize(640, 480);
glutCreateWindow("CUDA particles");
initGL();
init(numParticles, gridSize);
initParams();
initMenus();
if (benchmark)
{
if (numIterations <= 0)
numIterations = 300;
runBenchmark(numIterations);
}
else
{
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutMouseFunc(mouse);
glutMotionFunc(motion);
glutKeyboardFunc(key);
glutSpecialFunc(special);
glutIdleFunc(idle);
glutMainLoop();
}
if (psystem)
delete psystem;
cudaThreadExit();
return 0;
}
FlangerFilterInstance::FlangerFilterInstance(FlangerFilter *aParent)
{
mParent = aParent;
mBuffer = 0;
mBufferLength = 0;
mOffset = 0;
mIndex = 0;
initParams(3);
mParam[FlangerFilter::WET] = 1;
mParam[FlangerFilter::FREQ] = mParent->mFreq;
mParam[FlangerFilter::DELAY] = mParent->mDelay;
}
// class constructor
MatlabProtocolGil::MatlabProtocolGil(void)
: nh_(),
private_nh_("~")
{
// init parameters
initParams();
// init subscribers
initPublishers();
// init subscribers
initSubscribers();
}
SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB) :
JointSW(_arr, 2),
m_frameInA(frameInA),
m_frameInB(frameInB) {
A = rbA;
B = rbB;
A->add_constraint(this, 0);
B->add_constraint(this, 1);
initParams();
} // SliderJointSW::SliderJointSW()
SVMTrain::SVMTrain(const ParameterMap& params) : Analyzer(params) {
G_DEBUG(GAlgorithms, "Initializing SVMTrain analyzer");
validParams << "type" << "kernel" << "probability" << "degree" << "c"
<< "nu" << "gamma" << "className";
initParams();
_className = _params.value("className").toString();
// we never want to use the class descriptor to train an SVM (nonsense), so instead of
// complaining if it gets selected because the user used wildcards, just remove it silently
_exclude << _className;
}
static void *
nerverot_init (Display *dpy, Window window)
{
struct state *st = (struct state *) calloc (1, sizeof(*st));
st->dpy = dpy;
st->window = window;
initParams (st);
setup (st);
/* make a valid set to erase at first */
renderSegs (st);
return st;
}
void init()
{
wdt_start(wdt_250ms);
initProc();
initVars();
initParams();
#ifndef _DEBUG_
lcd_init(LCD_DISP_ON);
//init_lcd_simbols();
#ifdef _DEMO_VERSION_
SplashScreen();
#endif // _DEMO_VERSION_
#endif // _DEBUG_
SetMenu(&mPrograms);
}
void
Config::startup() {
initParams();
LoggerFactory::instance()->init(this);
log()->debug("logger factory started");
XmlUtils::init(this);
Loader::instance()->init(this);
log()->debug("loader started");
StatusInfo::instance()->init(this);
log()->debug("status info started");
Authorizer::instance()->init(this);
log()->debug("authorizer started");
Sanitizer::instance()->init(this);
log()->debug("sanitizer started");
ThreadPool::instance()->init(this);
log()->debug("thread pool started");
VirtualHostData::instance()->init(this);
log()->debug("virtual host data started");
ScriptCache::instance()->init(this);
log()->debug("script cache started");
ScriptFactory::instance()->init(this);
log()->debug("script factory started");
StylesheetCache::instance()->init(this);
log()->debug("stylesheet cache started");
StylesheetFactory::instance()->init(this);
log()->debug("stylesheet factory started");
ExtensionList::instance()->registerExtension(ExtensionHolder(new ControlExtension));
ExtensionList::instance()->init(this);
log()->debug("extension list started");
CacheStrategyCollector::instance()->init(this);
log()->debug("doc and page cache started");
}
