bool LoggingAvailability::startHook() { if (logger) { logger->info(RTT::rt_string("Available in startHook")); } else { log(Error) << "Not available in startHook()" << endlog(); } return true; }
bool Controller::startHook(){ if(current_pose_port.read(m_current_pose) == NoData){ log(Debug) << "No current pose received" << endlog(); return false; } else{ m_goal_pose.x = m_current_pose[0]; m_goal_pose.y = m_current_pose[1]; m_goal_pose.theta = m_current_pose[2]; return true; } }
/** * OperationCaller objects may be assigned to a part responsible for production * of an implementation. * This variant is used when a only a remote implementation is available. * * @param part The part used by the OperationCaller to produce an implementation. * * @return *this */ OperationCaller& operator=(OperationInterfacePart* part) { if (part == 0) { log(Warning) << "Assigning OperationCaller from null part."<<endlog(); this->impl.reset(); } if (this->impl && this->impl == part->getLocalOperation() ) return *this; OperationCaller<Signature> tmp(part); *this = tmp; return *this; }
/** * Create a Property \b mirroring another PropertyBase. * It copies the name and description, and \b shallow copies * the value. * @see ready() to inspect if the creation succeeded. */ Property( base::PropertyBase* source) : base::PropertyBase(source ? source->getName() : "", source ? source->getDescription() : ""), _value( source ? internal::AssignableDataSource<DataSourceType>::narrow(source->getDataSource().get() ) : 0 ) { if ( source && ! _value ) { log(Error) <<"Can not initialize Property from "<<source->getName() <<": "; if ( source->getDataSource() ) log() << "incompatible type ( destination type: "<< getType() << ", source type: "<< source->getDataSource()->getTypeName() << ")."<<endlog(); else log() << "source Property was not ready."<<endlog(); } }
/** * A Command objects may be assigned to an implementation. * If the implementation is of the wrong type, it is freed. * * @param implementation An implementation which will be owned * by the command. If it is unusable, it is freed. */ Command& operator=(base::DispatchInterface* implementation) { if ( this->impl && this->impl == implementation) return *this; delete this->impl; this->impl = dynamic_cast< base::CommandBase<CommandT>* >(implementation); if (this->impl == 0 && implementation) { log(Error) << "Tried to assign Command from incompatible type."<< endlog(); delete implementation; } return *this; }
void stopHook() { activate(false,internalNumber); //To write the .cpf property file // std::string propertyfile = "configuringXMLSlave" + intToStr(internalNumber)+".cpf"; // this->getProvider<Marshalling>("marshalling")->writeProperties(propertyfile); Logger::log(Logger::Info) << soem_slave::getName()<<" executes stopHook !" <<endlog(); }
bool configureHook() { minimumConfigure(); PDOmapping(); setting(); Logger::log(Logger::Info) << soem_slave::getName()<<" configured !" <<endlog(); return true; }
//read: read the value of one of the 2 channels in Volts /////////////////////////// double SoemEL3104::read(unsigned int chan) { if (chan < m_size) { return m_msg.values[chan]; } else log(Error) << "Channel " << chan << " is out of the module's range" << endlog(); return 0.0; }
RTT_CORBA_API bool ApplicationServer::InitOrb(int argc, char* argv[], Seconds timeout ) { if ( !CORBA::is_nil(orb) ){ return false; } try { // First initialize the ORB, that will remove some arguments... orb = CORBA::ORB_init (argc, const_cast<char**>(argv), "omniORB4"); if(timeout >= 0.1e-7) { #if defined( CORBA_IS_TAO ) && defined( CORBA_TAO_HAS_MESSAGING ) // Set the timeout value as a TimeBase::TimeT (100 nanosecond units) // and insert it into a CORBA::Any. TimeBase::TimeT relative_rt_timeout = timeout * 1.0e7; CORBA::Any relative_rt_timeout_as_any; relative_rt_timeout_as_any <<= relative_rt_timeout; // Create the policy and put it in a policy list. CORBA::PolicyList policies; policies.length(1); policies[0] = orb->create_policy (Messaging::RELATIVE_RT_TIMEOUT_POLICY_TYPE, relative_rt_timeout_as_any); // Apply the policy at the ORB level using the ORBPolicyManager. CORBA::Object_var obj = orb->resolve_initial_references ("ORBPolicyManager"); CORBA::PolicyManager_var policy_manager = CORBA::PolicyManager::_narrow (obj.in()); policy_manager->set_policy_overrides (policies, CORBA::SET_OVERRIDE); #else log(Error) << "Ignoring ORB timeout setting in non-TAO/Messaging build." <<endlog(); #endif // CORBA_IS_TAO } // Also activate the POA Manager, since we may get call-backs ! CORBA::Object_var poa_object = orb->resolve_initial_references ("RootPOA"); rootPOA = PortableServer::POA::_narrow (poa_object.in ()); PortableServer::POAManager_var poa_manager = rootPOA->the_POAManager (); poa_manager->activate (); return true; } catch (CORBA::Exception &e) { log(Error) << "Orb Init : CORBA exception raised!" << Logger::nl; Logger::log() << CORBA_EXCEPTION_INFO(e) << endlog(); std::cout << "ApplicationServer::InitOrb return false: ORBA exception raised\n"; } return false; }
virtual void serialize(const PropertyBag &v) { int retval; /** * Check if the netcdf file is already in define mode. * Increase counter every time serialize function is called and no errors occurs. */ if ( ncopen ) { ncopen++; } else { retval = nc_redef(ncid); if ( retval ) log(Error) << "Could not enter define mode in NetcdfHeaderMarshaller, error "<< retval <<endlog(); else ncopen++; } for ( PropertyBag::const_iterator i = v.getProperties().begin(); i != v.getProperties().end(); i++ ) { this->serialize(*i); } /** * Decrease counter, if zero enter data mode else stay in define mode */ if (--ncopen) log(Info) << "Serializer still in progress" <<endlog(); else { retval = nc_enddef(ncid); if (retval) log(Error) << "Could not leave define mode, error" << retval <<endlog(); } }
/** * Named OperationCaller objects may be looked up in a Service. * * @param service The Service where the operation will be looked up. * * @return *this */ OperationCaller& operator=(ServicePtr service) { if ( !service ) { log(Warning) << "Assigning OperationCaller from null service."<<endlog(); this->impl.reset(); } if (this->mname.empty()) { log(Error) << "Can't initialise unnamed OperationCaller from service '"<<service->getName() <<"'."<<endlog(); return *this; } OperationCaller<Signature> tmp(mname,service); *this = tmp; return *this; }
Calibrator::Calibrator(std::string name) : TaskContext(name,PreOperational) { log(Debug) << "(Calibrator) constructor entered" << endlog(); // Add properties addProperty("boardWidth",boardWidth).doc("number of internal corners in width"); addProperty("boardHeight",boardHeight).doc("number of internal corners in heigth"); addProperty("squareSize",squareSize).doc("size of square of the chessboard pattern"); addProperty("detectorSearchRadius",detectorSearchRadius).doc("radius for checkboard detector"); addProperty("imageDirectory",imageDirectory).doc("directory containing images for calibration"); // Add ports addPort( "extrinsicCameraParameters",_extrinsicPort ); // Add operations addOperation("batchCalibrate", &Calibrator::batchCalibrate, this).doc("Calibrate with batch images"); log(Debug) << "(Calibrator) constructor finished" << endlog(); }
// -------------------------- // Constructor for 3 modules // -------------------------- EncoderSSI_apci1710_board::EncoderSSI_apci1710_board( unsigned int mNr1, unsigned int mNr2, unsigned int mNr3 ) : PeriodicActivity( RTT::OS::HighestPriority, 1./ORONUM_DEVICE_DRIVERS_APCI1710_SSI_UPDATE ) , nr_of_modules(3), moduleNr1( mNr1 ), moduleNr2( mNr2 ), moduleNr3( mNr3 ) { buffer1 = new unsigned int[9]; buffer2 = new unsigned int[9]; readbuffer = buffer1; writebuffer = buffer2; log() << "(EncoderSSI_apci1710) Creating EncoderSSI..." << endlog(Info); dev = apci1710_get_device(); int res = 0; if ( 0 != (res = apci1710_ssi_init( dev, moduleNr1, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS, COUNTINGMODE_BINARY, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) ) //in Hz (25kHz is ok) log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr1 << " is not ready for SSI\n" << endlog(Error); if ( 0 != (res = apci1710_ssi_init( dev, moduleNr2, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS, COUNTINGMODE_BINARY, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) ) //in Hz (25kHz is ok) log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr2 << " is not ready for SSI\n" << endlog(Error); if ( 0 != (res = apci1710_ssi_init( dev, moduleNr3, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE2, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS2, COUNTINGMODE_BINARY, ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) ) //in Hz (25kHz is ok) log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr3 << " is not ready for SSI\n" << endlog(Error); #ifdef OROPKG_CORELIB_TIMING // init one buffer, display some statistics TimeService::ticks t=TimeService::Instance()->getTicks(); readCard( readbuffer ); t = TimeService::Instance()->ticksSince(t); TimeService::nsecs n = TimeService::ticks2nsecs(t); log() << "(EncoderSSI_apci1710) Reading SSI modules takes about " << long(n) << " nanoseconds" << endlog(Info); #endif this->start(); }
bool operator==(const std::vector<double>& a, const std::vector<double>& b) { if ( a.size() != b.size() ) { log(Error) << "Wrong vector sizes : " << a.size() <<" "<< b.size()<<endlog(); return false; } for(unsigned int i =0; i != a.size(); ++i) { if (a[i] != b[i]) { log(Error) << "Wrong vector element: "<<a[i]<<" != "<<b[i]<<" i:" << i<<endlog(); return false; } } return true; }
/** * Create a Property \b mirroring another PropertyBase. * It copies the name and description, and \b shallow copies * the value. * @see ready() to inspect if the creation succeeded. */ Property( base::PropertyBase* source) : base::PropertyBase(source ? source->getName() : "", source ? source->getDescription() : "") { if ( source ) { base::DataSourceBase::shared_ptr dsb = source->getDataSource(); if ( !setDataSource(dsb) ) { log(Error) << "Cannot initialize Property from " << source->getName() << ": "; if ( dsb ) { log() << "incompatible type ( destination type: " << getType() << ", source type: " << dsb->getTypeName() << ")." << endlog(); } else { log() << "source Property was not ready." << endlog(); } } } }
bool ArmBridgeRosOrocos::startHook() { m_cartesian_pose_with_impedance_ctrl_as->start(); m_joint_config_as->start(); //m_trajectory_as_srv->start(); //m_arm_has_active_joint_trajectory_goal = false; ROS_INFO("arm actions started"); if (!brics_joint_positions.connected()) { log(Error) << "BRICS joint positions not connected." << endlog(); return false; } if (!orocos_joint_positions.connected()) { log(Error) << "Orocos joint positions not connected." << endlog(); return false; } if (!orocos_homog_matrix.connected()) { log(Error) << "Orocos homog_matrix not connected." << endlog(); return false; } if (!orocos_arm_stiffness.connected()) { log(Error) << "arm stiffness port not connected." << endlog(); return false; } return TaskContext::startHook(); }
bool DataPool::addComponent(TaskContext* comp) { Logger::In in(getName()); log(Debug) << "Adding component " << comp->getName() << "." << endlog(); Ports ports = comp->ports()->getPorts(); for (Ports::iterator it = ports.begin(); it != ports.end() ; ++it) { RTT::base::PortInterface *port = *it; if (port->connected()) { log(Debug) << "Ignoring port " << comp->getName() << "/" << port->getName() << ": already connected" << endlog(); continue; } this->addPort(port); } return true; }
/** * Create a variable of data type float */ void store(Property<float> *v) { int retval; int varid; std::string sname = composeName(v->getName()); /** * Create a new variable with only one dimension i.e. the unlimited time dimension */ retval = nc_def_var(ncid, sname.c_str(), NC_FLOAT, DIMENSION_VAR, &dimsid, &varid); if ( retval ) log(Error) << "Could not create variable " << sname << ", error " << retval <<endlog(); else log(Info) << "Variable "<< sname << " successfully created" <<endlog(); }
bool OstreamAppender::configureHook() { // verify valid limits int m = maxEventsPerCycle_prop.rvalue(); if ((0 > m)) { log(Error) << "Invalid maxEventsPerCycle value of " << m << ". Value must be >= 0." << endlog(); return false; } maxEventsPerCycle = m; if (!appender) appender = new log4cpp::OstreamAppender(getName(), &std::cout); return configureLayout(); }
bool FileAppender::configureHook() { // verify valid limits int m = maxEventsPerCycle_prop.rvalue(); if ((0 > m)) { log(Error) << "Invalid maxEventsPerCycle value of " << m << ". Value must be >= 0." << endlog(); return false; } maxEventsPerCycle = m; // \todo error checking appender = new log4cpp::FileAppender(getName(), filename_prop.rvalue()); return configureLayout(); }
static bool createConnection(OutputPort<T>& output_port, base::InputPortInterface& input_port, ConnPolicy const& policy) { if ( !output_port.isLocal() ) { log(Error) << "Need a local OutputPort to create connections." <<endlog(); return false; } InputPort<T>* input_p = dynamic_cast<InputPort<T>*>(&input_port); // This is the input channel element of the output half base::ChannelElementBase::shared_ptr output_half = 0; if (input_port.isLocal() && policy.transport == 0) { // Local connection if (!input_p) { log(Error) << "Port " << input_port.getName() << " is not compatible with " << output_port.getName() << endlog(); return false; } // local ports, create buffer here. output_half = buildBufferedChannelOutput<T>(*input_p, output_port.getPortID(), policy, output_port.getLastWrittenValue()); } else { // if the input is not local, this is a pure remote connection, // if the input *is* local, the user requested to use a different transport // than plain memory, rare case, but we accept it. The unit tests use this for example // to test the OOB transports. if ( !input_port.isLocal() ) { output_half = createRemoteConnection( output_port, input_port, policy); } else output_half = createOutOfBandConnection<T>( output_port, *input_p, policy); } if (!output_half) return false; // Since output is local, buildChannelInput is local as well. // This this the input channel element of the whole connection base::ChannelElementBase::shared_ptr channel_input = buildChannelInput<T>(output_port, input_port.getPortID(), output_half); return createAndCheckConnection(output_port, input_port, channel_input, policy ); }
void YouBotBaseService::setControlModesAll(int mode) { for(int i=0;i<YouBot::NR_OF_BASE_SLAVES;++i) { m_joint_ctrl_modes[i] = static_cast<Hilas::ctrl_modes>(mode); switch (m_joint_ctrl_modes[i]) { case(Hilas::PLANE_ANGLE): { simxSetObjectIntParameter(clientID,vrep_joint_handle[i],VREP_JOINT_CONTROL_POSITION_IP,1,simx_opmode_oneshot); break; } case(Hilas::ANGULAR_VELOCITY): { simxSetObjectIntParameter(clientID,vrep_joint_handle[i],VREP_JOINT_CONTROL_POSITION_IP,0,simx_opmode_oneshot); break; } case(Hilas::TORQUE): { simxSetObjectIntParameter(clientID,vrep_joint_handle[i],VREP_JOINT_CONTROL_POSITION_IP,0,simx_opmode_oneshot); break; } case(Hilas::MOTOR_STOP): { simxSetObjectIntParameter(clientID,vrep_joint_handle[i],VREP_JOINT_CONTROL_POSITION_IP,1,simx_opmode_oneshot); break; } case(Hilas::TWIST): { simxSetObjectIntParameter(clientID,vrep_joint_handle[i],VREP_JOINT_CONTROL_POSITION_IP,0,simx_opmode_oneshot); break; } default: { log(Error) << "Case not recognized." << endlog(); this->getOwner()->error(); break; } } } }
void writelog(char* format, ...) { startlog(); if(logfile == 0) return; char buffer[0xFFF]; va_list args; va_start (args, format); vsprintf (buffer,format, args); va_end (args); strcat(buffer, "\n"); int len = strlen(buffer); fwrite(buffer, sizeof(char), len, logfile); endlog(); }
bool Calibrator::configureHook() { log(Debug) << "(Calibrator) configureHook entered" << endlog(); bool result = true; // create checkboard detector log(Debug) << "Create checkerboard detector " << endlog(); _detector = new CheckerboardDetector(boardWidth,boardHeight,squareSize); _detector->setFlags(CV_CALIB_CB_ADAPTIVE_THRESH); _detector->setSearchRadius(detectorSearchRadius); //_corners = vector<CvPoint2D32f>(_detector->corners() ); _corners.resize(_detector->corners()); // Create calibrator log(Debug) << "Create calibrator " << endlog(); _cal = new Calibrater(); _cal->setFlags(CV_CALIB_FIX_PRINCIPAL_POINT); cvNamedWindow("Calibration", 0); // Get files from directory of images _imageFiles = getDir(imageDirectory); // TODO: check this construction if (!result) { log(Error) << "failed to getDir " << imageDirectory << endlog(); } log(Debug) << "_imageFiles.size()" << _imageFiles.size( )<< endlog(); for (unsigned int i = 0;i < _imageFiles.size();i++) { log(Debug) << _imageFiles[i] << endlog(); } // Allocate memory for the extrinsic matrix _rot = cvCreateMat(_imageFiles.size(), 3, CV_32FC1); _trans = cvCreateMat(_imageFiles.size(), 3, CV_32FC1); // TODO: Write first time to _extrinsicPort here to allocate the port log(Debug) << "(Calibrator) configureHook finished" << endlog(); return result; }
bool Appender::configureLayout() { bool rc; const std::string& layoutName = layoutName_prop.rvalue(); const std::string& layoutPattern = layoutPattern_prop.rvalue(); rc = true; // prove otherwise if (appender && (!layoutName.empty())) { // \todo layout factory?? if (0 == layoutName.compare("basic")) { appender->setLayout(new log4cpp::BasicLayout()); } else if (0 == layoutName.compare("simple")) { appender->setLayout(new log4cpp::SimpleLayout()); } else if (0 == layoutName.compare("pattern")) { log4cpp::PatternLayout *layout = new log4cpp::PatternLayout(); /// \todo ensure "" != layoutPattern? layout->setConversionPattern(layoutPattern); appender->setLayout(layout); // the layout is now owned by the appender, and will be deleted // by it when the appender is destroyed } else { log(Error) << "Invalid layout '" << layoutName << "' in configuration for category: " << getName() << endlog(); rc = false; } } return rc; }
INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task, int priority, const char * name, int sched_type, size_t stack_size, void * (*start_routine)(void *), ThreadInterface* obj) { int rv; // return value rtos_task_check_priority( &sched_type, &priority ); // Save priority internally, since the pthread_attr* calls are broken ! // we will pick it up later in rtos_task_set_scheduler(). task->priority = priority; // Set name if ( strlen(name) == 0 ) name = "Thread"; task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name); if ( (rv = pthread_attr_init(&(task->attr))) != 0 ){ return rv; } // Set scheduler type (_before_ assigning priorities!) if ( (rv = pthread_attr_setschedpolicy(&(task->attr), sched_type)) != 0){ return rv; } pthread_attr_getschedpolicy(&(task->attr), &rv ); assert( rv == sched_type ); struct sched_param sp; sp.sched_priority=priority; // Set priority if ( (rv = pthread_attr_setschedparam(&(task->attr), &sp)) != 0 ){ return rv; } rv = pthread_create(&(task->thread), &(task->attr), start_routine, obj); log(Debug) <<"Created Posix thread "<< task->thread <<endlog(); return rv; }
void soem_SGDV::stopHook() { //Sending a last PDO to stop the motor target.Word=(unsigned short)CW_QUICK_STOP; target.Position=0; target.Speed=0; target.Torque=0; target.OpMode=9; //Sending to the master data it has to sent by EtherCAT about this slave memcpy(output,&target , RxPDOsize); activate(false,internalNumber); //To write the .cpf property file // std::string propertyfile = "configuringXMLSlave" + intToStr(internalNumber)+".cpf"; // this->getProvider<Marshalling>("marshalling")->writeProperties(propertyfile); Logger::log(Logger::Info) << soem_SGDV::getName()<<" executes stopHook !" <<endlog(); }
void Log::logv(Level l, ...) { if (l > lvl) return; bool first = true; const wchar_t *p; Tlsdata &tlsd(*tls); va_list vl; va_start(vl, l); while ((p = va_arg(vl, const wchar_t *)) != NULL) { if (first) first = false; else tlsd.strm << ' '; tlsd.strm << p; } va_end(vl); endlog(tlsd, l); }
void soem_SGDV::PDOmapping() { //Starting procedure for PDOmapping SDOwrite(0x1C12,00,false,1,0); SDOwrite(0x1C13,00,false,1,0); //PDOmapping of the TxPDO SDOwrite(0x1600,00,false,1,0); SDOwrite(0x1600,01,false,4,0x60400010); SDOwrite(0x1600,02,false,4,0x607A0020); SDOwrite(0x1600,03,false,4,0x60FF0020); SDOwrite(0x1600,04,false,4,0x60710010); SDOwrite(0x1600,05,false,4,0x60600008); //PDOmapping of the RxPDO SDOwrite(0x1A00,00,false,1,0); SDOwrite(0x1A00,01,false,4,0x60410010); SDOwrite(0x1A00,02,false,4,0x60640020); SDOwrite(0x1A00,03,false,4,0x606C0020); SDOwrite(0x1A00,04,false,4,0x60770010); SDOwrite(0x1A00,05,false,4,0x60610008); //Ending procedure for PDOmapping SDOwrite(0x1600,00,false,1,5); SDOwrite(0x1A00,00,false,1,5); SDOwrite(0x1C12,01,false,2,0x1600); SDOwrite(0x1C13,01,false,2,0x1A00); SDOwrite(0x1C12,00,false,1,1); SDOwrite(0x1C13,00,false,1,1); Logger::log(Logger::Info) << soem_SGDV::getName()<<" Executed PDO mapping !" <<endlog(); }
unsigned int EncoderSSI_apci1710_board::read( unsigned int encNr ) { #ifdef DEBUG if ( encNr < 1 || encNr > nr_of_modules * 3 ) log() << "(EncoderSSI_apci1710) ERROR: encNr < 1 or encNr > " << nr_of_modules * 3 << "!" << endlog(Error); #endif OS::MutexLock Locker( readLock ); return readbuffer[ encNr - 1 ]; }