/// Solve the problem
void p_solve(const p_optimizeMethod& m)
{
switch (m) {
case (Maximize):
p_impl->maximize();
break;
case (Minimize):
p_impl->minimize();
break;
default:
BOOST_ASSERT(false);
}
}
virtual void handle_start()
{
tcp::resolver resolver(main_pool().io_service());
tcp::endpoint endpoint = *resolver.resolve(addr_query_);
acceptor_.reset(new tcp::acceptor(main_pool().io_service()));
acceptor_->open(endpoint.protocol());
acceptor_->set_option(tcp::acceptor::reuse_address(true));
acceptor_->bind(endpoint);
acceptor_->listen();
start_accept();
std::cout << "Server started" << std::endl;
}
bool CameraV4LPublisher::getImage(cv::Mat& img, bool emptyBuffer)
{
bool ok = false;
if ( emptyBuffer )
{
for (int i = 0; i < 4; ++i) //v4l stores a buffer of 4 images
_capture->grab();
}
if ( _capture->grab() )
if ( _capture->retrieve(img) )
ok = true;
return ok;
}
/*! @brief configure the detector state
*
* The configure method is called once when the ECTO cell is launched,
* and is designed to initialise the detector state and load models,
* parameters, etc.
*
* @param params the parameters made available through the config file and
* python bindings
* @param inputs for initializing inputs, if necessary
* @param outputs for initializing outputs, if necessary
*/
void
configure(const tendrils& params, const tendrils& inputs, const tendrils& outputs) {
// create the model object and deserialize it
FileStorageModel model;
model.deserialize(*model_file_);
// create the visualizer
visualizer_.reset(new Visualize(model.name()));
// create the PartsBasedDetector and distribute the model parameters
detector_.reset(new PartsBasedDetector<float>);
detector_->distributeModel(model);
}
VisualServo() : arm_("left"),
planning_group_name_(arm_+"_arm"),
camera_frame_(arm_+"_hand_camera")
{
move_group_.reset(new move_group_interface::MoveGroup(planning_group_name_));
move_group_->setPlanningTime(30.0);
move_group_->setPlannerId("RRTstark");
while(ros::ok())
{
// marker_pose_subscriber_ = node_.subscribe("ar_pose_marker",50, &VisualServo::marker_pose_callback, this);
// target_publisher_ = node_.advertise<geometry_msgs::Pose>("/visual_servo_target", 10);
servo_to_tag();
}
}
void reset()
{
fallback.reset();
effects.clear();
if(target)
target.Release();
if(offsetTarget)
offsetTarget.Release();
vbuffer.reset();
ibuffer.reset();
vshader.reset();
pshader.reset();
}
/** allocs a scoped_block for a new key
** doesn't support reallocing on an existing key, maybe in the future */
void alloc(boost::scoped_ptr< variable_store::scoped_block > & p,
const Key & key,
size_t block_size)
{
boost::mutex::scoped_lock lock(m_mutex);
// don't support reallocing for now
typename hm_key_location::const_iterator it = m_key_location.find(key);
if (it != m_key_location.end())
throw std::invalid_argument("realloc not supported");
p.reset( new variable_store::scoped_block(m_variable_store, block_size) );
m_key_location[key] = location(p->block_size(), p->index());
}
~FiberControl(){
AUTO(it, g_stack_pool.begin());
for(;;){
if(it == g_stack_pool.end()){
stack.reset();
break;
}
if(!*it){
stack.swap(*it);
break;
}
++it;
}
}
void HeartbeatArrival(uint16_t sourceId, const std::uint8_t *data, std::uint8_t len)
{
if (len<5)
return;
CANExtended::DeviceState state = static_cast<CANExtended::DeviceState>(data[0]);
if (state != CANExtended::Operational)
return;
auto unit = unitManager->FindUnit(sourceId&0x7f);
bool updated = (unit!=nullptr) && (unit->UpdateStatus()==StorageUnit::Updated);
if (unit == nullptr || updated)
{
StorageBasic basic(CanEx);
basic.DeviceId = sourceId;
basic.SensorNum = data[2];
basic.Version = (data[3]<<8)|data[4];
switch (data[1])
{
case UNIT_TYPE_INDEPENDENT:
unit.reset(new IndependentUnit(basic));
break;
case UNIT_TYPE_UNITY:
unit.reset(new UnityUnit(basic));
break;
case UNIT_TYPE_UNITY_RFID:
unit.reset(new RfidUnit(basic));
break;
default:
CanEx->Sync(sourceId, DeviceSync::SyncLive, CANExtended::Trigger);
return;
}
unit->ReadCommandResponse.bind(ethEngine.get(), &NetworkEngine::DeviceReadResponse);
unit->WriteCommandResponse.bind(ethEngine.get(), &NetworkEngine::DeviceWriteResponse);
CanEx->RegisterDevice(unit);
if (updated)
{
unitManager->Recover(sourceId&0x7f, unit);
#ifdef DEBUG_PRINT
cout<<"#Recovered Device 0x"<<std::hex<<sourceId<<std::dec<<endl;
#endif
}
else
{
unitManager->Add(sourceId&0x7f, unit);
#ifdef DEBUG_PRINT
cout<<"#Added Device 0x"<<std::hex<<sourceId<<std::dec<<endl;
#endif
}
}
//CanEx->Sync(sourceId, DeviceSync::SyncLive, CANExtended::AutoSync); //Confirm & Start AutoSync
CanEx->Sync(sourceId, DeviceSync::SyncLive, CANExtended::Trigger);
if (updated)
unitManager->SyncUpdate();
}
//-----------------------------------------------------------------------------
void Editor::Initialize(Main::IGame* pGame)
{
m_pGame = pGame;
Core::IGameStateManager* pStateMan = m_pGame->GetGameStateManager();
Main::GameStateTest::Create(pStateMan, "StateTest");
pStateMan->Change(pStateMan->FindByName("StateTest"));
m_pGrid.reset(m_pGame->GetEngine()->CreateGrid());
m_pGrid->Initialze(100.f, 1.f);
m_pCamManipulator.reset(m_pGame->GetEngine()->CreateCameraManipulator());
m_pCamManipulator->Set(Ogre::Vector3(-50.f, 0.f, 50.f),Ogre::Vector3(0,0,0));
}
~OmniNamesFixture() {
BOOST_TEST_MESSAGE("== Test teardown [BEGIN]: Stopping OmniNames ==");
bf::remove_all(logdir);
if (processNamingService) {
try {
processNamingService->terminate();
processNamingService->wait();
} catch (...) {
BOOST_TEST_MESSAGE("== Problem while ending OmniNames ==");
}
}
boost::this_thread::sleep(boost::posix_time::milliseconds(SLEEP_TIME));
BOOST_TEST_MESSAGE("== Test teardown [END]: Stopping OmniNames ==");
}
sound::InputStream*
Sound_as::attachAuxStreamerIfNeeded()
{
media::AudioInfo* audioInfo = _mediaParser->getAudioInfo();
if (!audioInfo) return 0;
// the following may throw an exception
_audioDecoder.reset(_mediaHandler->createAudioDecoder(*audioInfo).release());
// start playing ASAP, a call to ::start will just change _startTime
#ifdef GNASH_DEBUG_SOUND_AS
log_debug("Attaching the aux streamer");
#endif
return _soundHandler->attach_aux_streamer(getAudioWrapper, (void*) this);
}
boost::shared_ptr<mongo::DBClientCursor> query(const std::string &json, int limit = 0, int skip = 0) {
try {
mongo::DBClientCursor *ptr = conn_->get()->query(ns_, mongo::Query(json), limit, skip).release();
if (!ptr)
throw conn_->get()->getLastError();
return boost::shared_ptr<mongo::DBClientCursor>(ptr);
} catch(mongo::DBException &de) {
std::string err_msg = "Mongodb Plugin: ";
err_msg += de.toString();
err_msg += "\n";
throw mapnik::datasource_exception(err_msg);
}
}
static void update_timestamp( bool force = false )
{
static const boost::posix_time::time_duration timeout = boost::posix_time::seconds( 60 );
static boost::posix_time::ptime last = boost::posix_time::microsec_clock::universal_time();
boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();
if( !force && ( now - last ) < timeout ) { return; }
if( verbose ) { std::cerr << "sick-ldmrs-stream: setting time to " << boost::posix_time::to_iso_string( now ) << "..." << std::endl; }
std::pair< comma::uint32, comma::uint32 > ntp = snark::timing::to_ntp_time( now );
clear_fault();
if( !protocol->write( sick::ibeo::commands::set_ntp_seconds( ntp.first ) ).ok() ) { COMMA_THROW( comma::exception, "failed to set NTP seconds" ); }
clear_fault();
if( !protocol->write( sick::ibeo::commands::set_ntp_fractions( ntp.second ) ).ok() ) { COMMA_THROW( comma::exception, "failed to set NTP fractions" ); }
last = now;
if( verbose ) { std::cerr << "sick-ldmrs-stream: set time to " << boost::posix_time::to_iso_string( now ) << std::endl; }
}
void train(DataView2D<FeatureType> samples,
DataView2D<int> labels,
int* sample_idxes,
size_t n_samples,
SplitGenerator split_generator,
RandomGen const& gen
)
{
// copy generator
RandomGen my_gen = gen;
// initialize root node
root_node.reset(new NodeT(0, params));
// train root node (other notes get trained recursively)
root_node->train(samples, labels, sample_idxes, n_samples, split_generator, my_gen);
}
static void Traversal(void const *argument) //Prevent missing status
{
static bool forceReport = true;
while(1)
{
if (!ethEngine->IsConnected())
{
forceReport = true;
continue;
}
unitManager->Traversal(forceReport); //Update all units
forceReport = false;
osThreadYield();
}
}
LogBackend& LogBackend::instance()
{
static boost::scoped_ptr< LogBackend > m_t;
static boost::mutex m_mutex;
static bool m_initialized = false;
if ( !m_initialized ) {
boost::lock_guard< boost::mutex > lock( m_mutex );
if ( !m_initialized ) {
m_t.reset( new LogBackend() );
m_initialized = true;
}
}
return *m_t;
}
virtual bool last() {
current = end;
while (current != begin) {
--current;
nested_current.reset((*current)->lines());
if (nested_current->last()) {
valid = true;
return true;
}
}
valid = false;
return false;
}
static bool clear_fault()
{
boost::optional< sick::ibeo::fault > fault = protocol->last_fault();
if( !fault ) { return true; }
std::cerr << "sick-ldmrs-stream: " << *fault << std::endl;
return !fault->fatal();
}
float similarity_path(float const* sample_1, float const* sample_2) {
NodeT const* node_1 = root_node.get();
NodeT const* node_2 = root_node.get();
int n_common = 0;
int n_1 = 1;
int n_2 = 1;
while (node_1 == node_2) {
if (node_1->is_leaf) {
return 1;
}
n_common++;
node_1 = node_1->split(sample_1);
node_2 = node_2->split(sample_2);
}
while (!node_1->is_leaf) {
node_1 = node_1->split(sample_1);
n_1++;
}
while (!node_2->is_leaf) {
node_2 = node_2->split(sample_2);
n_2++;
}
return n_common/sqrt((n_common + n_1)*(n_common + n_2));
}
bool set_disabled(SetDisabledRequest &request, SetDisabledResponse &response) {
disabled = request.disabled;
if (disabled) {
c3trajectory.reset();
}
return true;
}
GeneralPlane& operator=( const GeneralPlane& rhs ) {
if(&rhs!=this){
shape_.reset(rhs.shape_->clone());
traits_=rhs.traits_;
}
return *this;
}
int main( int ac, char** av )
{
try
{
comma::command_line_options options( ac, av );
if( options.exists( "--help,-h,--long-help" ) ) { usage(); }
verbose = options.exists( "--verbose,-v" );
stdin_csv = comma::csv::options( options );
if( stdin_csv.binary() )
{
std::pair< entry_t, comma::csv::options > p = make_input_( stdin_csv );
input = p.first;
stdin_csv = p.second;
istream.reset( new comma::csv::input_stream< entry_t >( std::cin, stdin_csv, input ) );
if( verbose ) { std::cerr << "csv-calc: input fields: " << stdin_csv.fields << std::endl; } // todo: remove
}
std::vector< std::string > unnamed = options.unnamed( "--verbose,-v", "--binary,-b,--delimiter,-d,--fields,-f" );
if( unnamed.empty() ) { std::cerr << "csv-calc: please specify operations" << std::endl; return 1; }
std::vector< std::string > operation_strings = comma::split( unnamed[0], ',' );
return 0;
}
catch( std::exception& ex )
{
std::cerr << "csv-calc: " << ex.what() << std::endl;
}
catch( ... )
{
std::cerr << "csv-calc: unknown exception" << std::endl;
}
return 1;
}
void HeatbeatTimerCallback(void const *arg)
{
static uint8_t hbcount = 20;
static int warmCount = 0;
HAL_GPIO_TogglePin(STATUS_PIN);
//Send a heart beat per 10s
if (++hbcount>20) // 20*500ms = 10s/hb
{
RequestTemperature();
#ifdef DEBUG_PRINT
cout<<"Current Temperature: "<<CurrentTemperature<<endl;
#endif
if (warmCount<WARM_LIMIT)
warmCount++;
if (CanEx != nullptr && !UnitManager::IsUpdating())
{
float t=CurrentTemperature - warmCount*WARM_COEFF;
StorageUnit::SetTemperature(*CanEx, t);
#ifdef DEBUG_PRINT
cout<<"Calibrated Temperature: "<<t<<endl;
#endif
}
hbcount = 0;
if (ethEngine!= nullptr)
ethEngine->SendHeartBeat();
}
}
inline void MeanModel::addNewPoint(const vectord &x)
{
using boost::numeric::ublas::column;
mFeatM.resize(mFeatM.size1(),mFeatM.size2()+1);
column(mFeatM,mFeatM.size2()-1) = mMean->getFeatures(x);
}
CGameWindow()
: CFKWindow( 640,480,false )
, m_Player( Graphics(), Audio() )
, m_Font( Graphics(), FK2DEngine::DefaultFontName(), 24 )
, m_nFps( 0 )
, m_nLastFps( 0 )
{
SetCaption( L"自由骑士笃志引擎：DEMO1" );
std::wstring szFileName = FK2DEngine::ShareResourcePrefix() + L"rc/media/Space.png";
m_pBackgroundImage.reset( new FK2DEngine::CImage( Graphics(), szFileName, false ) );
szFileName = FK2DEngine::ShareResourcePrefix() + L"rc/media/Star.png";
FK2DEngine::ImagesFromTiledBitmap( Graphics(), szFileName, 25, 25, false, m_StarAnim );
m_Player.Warp( 320, 240 );
//szFileName = FK2DEngine::ShareResourcePrefix() + L"avgRc\\Attack.cur";
//SetNewCursor( szFileName );
szFileName = FK2DEngine::ShareResourcePrefix() + L"avgRc\\AnimCursor.bmp";
m_pCursorBitmap = LoadImageFile( szFileName );
SSAnimationCursorManager::Instance()->AddCursor( m_pCursorBitmap, L"默认鼠标", 32, 32 );
SSAnimationCursorManager::Instance()->SetAnimCursor( L"默认鼠标" );
SSAnimationCursorManager::Instance()->Enable( true );
}
void normalizeExposure()
{
ROS_INFO("Normalizing exposure");
/// @todo assert(stopped)
last_exposure_value_ = 0;
consecutive_stable_exposures_ = 0;
cam_->setFrameCallback(boost::bind(&ProsilicaNode::normalizeCallback, this, _1));
cam_->start(prosilica::Freerun);
/// @todo thread safety
while (consecutive_stable_exposures_ < 3)
boost::this_thread::sleep(boost::posix_time::millisec(250));
cam_->stop();
}
static Pair capture( snark::camera::dc1394& camera )
{
static comma::signal_flag is_shutdown;
if( is_shutdown ) { reader->stop(); return Pair(); }
cv::Mat image = camera.read();
return std::make_pair( camera.time(), image.clone() );
}
// Recieve Action Goal Function
void processGripperAction(const clam_msgs::ClamGripperCommandGoalConstPtr& goal)
{
goal_ = goal;
if( doGripperAction(goal) )
{
result_.reached_goal = true;
action_server_->setSucceeded(result_);
}
else
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to complete gripper action");
result_.reached_goal = false;
action_server_->setSucceeded(result_);
}
}
void objectUpdated(Object* object)
{
if (currentStepObject && *currentStepObject == *object) {
currentStepObject.reset(object->clone());
if (isCurrentStepDone() && !currentStepRecorded) {
inputPoints.push_back(currentStepObject->getPosition());
outputPoints.push_back(convert(getCurrentStepPosition()));
currentStepRecorded = true;
LOG4CPLUS_INFO(
logger,
"Obtained calibration point " << currentStep + 1 << ": " << currentStepObject->getPosition() << " -> " << convert(getCurrentStepPosition()));
}
}
}
