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();
        }
      }

     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 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();
}

      /**
       * 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;
    }