<commit_before>#include "t200_thruster.h"
//#include "sub_thruster_library/seabotix_thruster.h"
#include "generic_thruster.h"
#include "sub_trajectory/ThrusterCmd.h"
#include <json/json.h>

#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/SelfTest.h> //TODO: implement self tests
#include <self_test/self_test.h>

#include <iostream>
#include <fstream>
#include <memory>

inline const std::string BoolToString(const bool b); //http://stackoverflow.com/a/29798

class ThrusterManager {
    ros::NodeHandle nh_;
    ros::Subscriber command_subscriber;
    ros::Publisher diagnostics_output;
    self_test::TestRunner self_test_;

    sub_trajectory::ThrusterCmd savedMsg;

    std::map<int, std::unique_ptr<GenericThruster>> thrusterMap;

    int updateRate;
    int expectedThrusters;

    ros::ServiceServer initServer;

public:
    ThrusterManager() : self_test_()
    {
        for(int i = 0; i < 8; i++) {
            savedMsg.cmd.push_back(0.0);
        }

        command_subscriber = nh_.subscribe("/thrusters/cmd_vel", 1000, &ThrusterManager::thrusterCb, this);

        diagnostics_output = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1000);

        self_test_.add("Test connections", this, &ThrusterManager::testThrusterConnections);

        initServer = nh_.advertiseService("initThrusters", &ThrusterManager::initService, this);

    	nh_.param("/updateRate", updateRate, 30);
    }

    Json::Value loadConfig(std::string filename)
    {
        ifstream configFile(filename);
        if(!configFile.is_open()){
            ROS_ERROR("Thruster controller couldn't open config file");
            //status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Config file didn't load");
            Json::Value obj;
            return obj;
        }

        ROS_INFO("Thruster controller config loading");
        Json::Value obj;
        configFile >> obj;
        ROS_INFO("Thruster controller config loaded");
        return obj;
    }
    bool initService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
    {
        init();
        return true;
    }

    void init()
    {
        thrusterMap.clear();
        Json::Value thrustersJson = loadConfig("config.json")["COMPUTE"];

        savedMsg = sub_trajectory::ThrusterCmd();
        savedMsg.cmd.resize(thrustersJson.size(), 0.0);
        expectedThrusters = thrustersJson.size();

        diagnostic_msgs::DiagnosticArray diag;

        for(int i = 0; i < thrustersJson.size(); i++) {
            int thrusterID = thrustersJson[i]["ID"].asInt();
            int thrusterType = thrustersJson[i]["Type"].asInt(); //TODO: support for multiple thruster types
            int thrusterAddress = thrustersJson[i]["Address"].asInt();
            ROS_INFO("Initializing thruster %d", thrusterID);
            try{
                  thrusterMap[i] = std::unique_ptr<GenericThruster>(new T200Thruster(1,thrusterAddress));
            }
            catch(I2CException e){
                //If we get here there's a bus problem
                ROS_ERROR("I2C error while connecting to thruster address %x", thrusterAddress);
                thrusterMap.erase(i);
                //Publish an error message for the diagnostic system to do something about
                diagnostic_msgs::DiagnosticStatus status;
                status.name = "Thrusters";
                status.hardware_id = "Thrusters_"+thrustersJson[i]["Address"].asString();
                status.level = status.ERROR;
                diag.status.push_back(status);
            }
        }
        diagnostics_output.publish(diag);
        ros::spinOnce();
        ROS_INFO("Done initializing thrusters");
    }

    void spin()
    {
        ros::Rate rate(updateRate);
        int loopCount;
        while(ros::ok()) {
            diagnostic_msgs::DiagnosticArray diag;
            ROS_DEBUG("Updating thrusters");
            if(loopCount++ > updateRate*2 && thrusterMap.size() < expectedThrusters)
            {
                init();
                loopCount = 0;
            }

            if(thrusterMap.size() != savedMsg.cmd.size())
            {
                diagnostic_msgs::DiagnosticStatus status;
                status.name = "Thrusters";
                status.hardware_id = "Thrusters";
                status.level = status.ERROR;
                diag.status.push_back(status);
                ROS_ERROR("Thrusters command has wrong number of values");
            }
            else
            {
                for(auto& iter:thrusterMap)
                {
                    diagnostic_msgs::DiagnosticStatus status;
                    status.name = "Thrusters";
                    status.hardware_id = "Thruster_"+std::to_string(iter.first);

                    try {
                        iter.second->updateStatus();
                        iter.second->setVelocityRatio(savedMsg.cmd.at(iter.first));
                    } catch(I2CException e) {
                        //Publish an error message for the diagnostic system to do something about
                        status.level = status.ERROR;
                    } catch (std::out_of_range e) {
                        ROS_ERROR("Thrusters command has not enough values");
                    }

                    if (thrusterOk(iter.second) && status.level != status.ERROR)
                        status.level = status.OK;
                    else
                        status.level = status.ERROR;

                    PushDiagData(status, iter.second, std::to_string(iter.first));
                    diag.status.push_back(status);
                }
            }

            diagnostics_output.publish(diag);
            ros::spinOnce();
            self_test_.checkTest();
            rate.sleep();
        }
    }

    bool thrusterOk (std::unique_ptr<GenericThruster> & thruster)
    {
        return thruster->isAlive() && thruster->inLimits();
    }

    void PushDiagData(diagnostic_msgs::DiagnosticStatus & statusmsg, std::unique_ptr<GenericThruster> & thruster, std::string thrusterName)
    {
        diagnostic_msgs::KeyValue thrusterValue;

        thrusterValue.key = "Thruster Type";
        thrusterValue.value = thruster->getType();
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Alive";
        thrusterValue.value = BoolToString(thruster->isAlive());
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Voltage";
        thrusterValue.value = std::to_string(thruster->getVoltage());
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Current";
        thrusterValue.value = std::to_string(thruster->getCurrent());
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Temperature";
        thrusterValue.value = std::to_string(thruster->getTemperature());
        statusmsg.values.push_back(thrusterValue);
    }

    void thrusterCb(const sub_trajectory::ThrusterCmd &msg)
    {
		savedMsg = msg;
    }

    //Self test function
    void testThrusterConnections(diagnostic_updater::DiagnosticStatusWrapper& status)
    {
        self_test_.setID("thrusterController");
        std::stringstream failedThrusters;
        Json::Value& thrustersJson = loadConfig("config.json")["COMPUTE"];
        for(int i = 0; i < thrustersJson.size(); i++)
		{
            int thrusterID = thrustersJson[i]["ID"].asInt();
            int thrusterType = thrustersJson[i]["Type"].asInt(); //TODO: support for multiple thruster types
            int thrusterAddress = thrustersJson[i]["Address"].asInt();

            try{
                T200Thruster(1, thrusterAddress).updateStatus();
            }
            catch(I2CException e){
                failedThrusters << thrusterAddress << " ";
            }
        }

        if(failedThrusters.str().length() > 0){
            status.add("Failures", failedThrusters.str());
            status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Thruster couldn't connect");
        } else {
            status.summary(diagnostic_msgs::DiagnosticStatus::OK, "All thrusters OK");
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "thruster_driver");
    ThrusterManager tc;
    tc.init();
    ROS_INFO("Thruster controller initialized, spinning");
    tc.spin();
    return 0;
}

inline const std::string BoolToString(const bool b)
{
  return b ? "true" : "false";
}
<commit_msg>Set different names for each thruster in diagnostics<commit_after>#include "t200_thruster.h"
//#include "sub_thruster_library/seabotix_thruster.h"
#include "generic_thruster.h"
#include "sub_trajectory/ThrusterCmd.h"
#include <json/json.h>

#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/SelfTest.h> //TODO: implement self tests
#include <self_test/self_test.h>

#include <iostream>
#include <fstream>
#include <memory>

inline const std::string BoolToString(const bool b); //http://stackoverflow.com/a/29798

class ThrusterManager {
    ros::NodeHandle nh_;
    ros::Subscriber command_subscriber;
    ros::Publisher diagnostics_output;
    self_test::TestRunner self_test_;

    sub_trajectory::ThrusterCmd savedMsg;

    std::map<int, std::unique_ptr<GenericThruster>> thrusterMap;

    int updateRate;
    int expectedThrusters;

    ros::ServiceServer initServer;

public:
    ThrusterManager() : self_test_()
    {
        for(int i = 0; i < 8; i++) {
            savedMsg.cmd.push_back(0.0);
        }

        command_subscriber = nh_.subscribe("/thrusters/cmd_vel", 1000, &ThrusterManager::thrusterCb, this);

        diagnostics_output = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1000);

        self_test_.add("Test connections", this, &ThrusterManager::testThrusterConnections);

        initServer = nh_.advertiseService("initThrusters", &ThrusterManager::initService, this);

    	nh_.param("/updateRate", updateRate, 30);
    }

    Json::Value loadConfig(std::string filename)
    {
        ifstream configFile(filename);
        if(!configFile.is_open()){
            ROS_ERROR("Thruster controller couldn't open config file");
            //status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Config file didn't load");
            Json::Value obj;
            return obj;
        }

        ROS_INFO("Thruster controller config loading");
        Json::Value obj;
        configFile >> obj;
        ROS_INFO("Thruster controller config loaded");
        return obj;
    }
    bool initService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
    {
        init();
        return true;
    }

    void init()
    {
        thrusterMap.clear();
        Json::Value thrustersJson = loadConfig("config.json")["COMPUTE"];

        savedMsg = sub_trajectory::ThrusterCmd();
        savedMsg.cmd.resize(thrustersJson.size(), 0.0);
        expectedThrusters = thrustersJson.size();

        diagnostic_msgs::DiagnosticArray diag;

        for(int i = 0; i < thrustersJson.size(); i++) {
            int thrusterID = thrustersJson[i]["ID"].asInt();
            int thrusterType = thrustersJson[i]["Type"].asInt(); //TODO: support for multiple thruster types
            int thrusterAddress = thrustersJson[i]["Address"].asInt();
            ROS_INFO("Initializing thruster %d", thrusterID);
            try{
                  thrusterMap[i] = std::unique_ptr<GenericThruster>(new T200Thruster(1,thrusterAddress));
            }
            catch(I2CException e){
                //If we get here there's a bus problem
                ROS_ERROR("I2C error while connecting to thruster address %x", thrusterAddress);
                thrusterMap.erase(i);
                //Publish an error message for the diagnostic system to do something about
                diagnostic_msgs::DiagnosticStatus status;
                status.name = "Thruster_"+thrustersJson[i]["Address"].asString();
                status.hardware_id = "Thruster_"+thrustersJson[i]["Address"].asString();
                status.level = status.ERROR;
                diag.status.push_back(status);
            }
        }
        diagnostics_output.publish(diag);
        ros::spinOnce();
        ROS_INFO("Done initializing thrusters");
    }

    void spin()
    {
        ros::Rate rate(updateRate);
        int loopCount;
        while(ros::ok()) {
            diagnostic_msgs::DiagnosticArray diag;
            ROS_DEBUG("Updating thrusters");
            if(loopCount++ > updateRate*2 && thrusterMap.size() < expectedThrusters)
            {
                init();
                loopCount = 0;
            }

            if(thrusterMap.size() != savedMsg.cmd.size())
            {
                diagnostic_msgs::DiagnosticStatus status;
                status.name = "Thrusters";
                status.hardware_id = "Thrusters";
                status.level = status.ERROR;
                diag.status.push_back(status);
                ROS_ERROR("Thrusters command has wrong number of values");
            }
            else
            {
                for(auto& iter:thrusterMap)
                {
                    diagnostic_msgs::DiagnosticStatus status;
                    status.name = "Thruster_"+std::to_string(iter.first);
                    status.hardware_id = "Thruster_"+std::to_string(iter.first);

                    try {
                        iter.second->updateStatus();
                        iter.second->setVelocityRatio(savedMsg.cmd.at(iter.first));
                    } catch(I2CException e) {
                        //Publish an error message for the diagnostic system to do something about
                        status.level = status.ERROR;
                    } catch (std::out_of_range e) {
                        ROS_ERROR("Thrusters command has not enough values");
                    }

                    if (thrusterOk(iter.second) && status.level != status.ERROR)
                        status.level = status.OK;
                    else
                        status.level = status.ERROR;

                    PushDiagData(status, iter.second, std::to_string(iter.first));
                    diag.status.push_back(status);
                }
            }

            diagnostics_output.publish(diag);
            ros::spinOnce();
            self_test_.checkTest();
            rate.sleep();
        }
    }

    bool thrusterOk (std::unique_ptr<GenericThruster> & thruster)
    {
        return thruster->isAlive() && thruster->inLimits();
    }

    void PushDiagData(diagnostic_msgs::DiagnosticStatus & statusmsg, std::unique_ptr<GenericThruster> & thruster, std::string thrusterName)
    {
        diagnostic_msgs::KeyValue thrusterValue;

        thrusterValue.key = "Thruster Type";
        thrusterValue.value = thruster->getType();
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Alive";
        thrusterValue.value = BoolToString(thruster->isAlive());
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Voltage";
        thrusterValue.value = std::to_string(thruster->getVoltage());
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Current";
        thrusterValue.value = std::to_string(thruster->getCurrent());
        statusmsg.values.push_back(thrusterValue);

        thrusterValue.key = thrusterName + " Temperature";
        thrusterValue.value = std::to_string(thruster->getTemperature());
        statusmsg.values.push_back(thrusterValue);
    }

    void thrusterCb(const sub_trajectory::ThrusterCmd &msg)
    {
		savedMsg = msg;
    }

    //Self test function
    void testThrusterConnections(diagnostic_updater::DiagnosticStatusWrapper& status)
    {
        self_test_.setID("thrusterController");
        std::stringstream failedThrusters;
        Json::Value& thrustersJson = loadConfig("config.json")["COMPUTE"];
        for(int i = 0; i < thrustersJson.size(); i++)
		{
            int thrusterID = thrustersJson[i]["ID"].asInt();
            int thrusterType = thrustersJson[i]["Type"].asInt(); //TODO: support for multiple thruster types
            int thrusterAddress = thrustersJson[i]["Address"].asInt();

            try{
                T200Thruster(1, thrusterAddress).updateStatus();
            }
            catch(I2CException e){
                failedThrusters << thrusterAddress << " ";
            }
        }

        if(failedThrusters.str().length() > 0){
            status.add("Failures", failedThrusters.str());
            status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Thruster couldn't connect");
        } else {
            status.summary(diagnostic_msgs::DiagnosticStatus::OK, "All thrusters OK");
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "thruster_driver");
    ThrusterManager tc;
    tc.init();
    ROS_INFO("Thruster controller initialized, spinning");
    tc.spin();
    return 0;
}

inline const std::string BoolToString(const bool b)
{
  return b ? "true" : "false";
}
<|endoftext|>


<commit_before>// Bindings
#include "PyROOT.h"
#include "Adapters.h"
#include "Utility.h"

// ROOT
#include "TInterpreter.h"
#include "TBaseClass.h"
#include "TClass.h"
#include "TClassEdit.h"
#include "TDataType.h"
#include "TDataMember.h"
#include "TMethod.h"
#include "TFunction.h"
#include "TMethodArg.h"
#include "TList.h"
#include "TError.h"

//= TReturnTypeAdapter =======================================================
std::string PyROOT::TReturnTypeAdapter::Name( unsigned int mod ) const
{
// get the name of the return type that is being adapted
   std::string name = fName;

   if ( ! ( mod & Rflx::QUALIFIED ) )
      name = TClassEdit::CleanType( fName.c_str(), 1 );

   if ( mod & Rflx::FINAL )
      name = Utility::ResolveTypedef( name );

   return name;
}

//= TMemberAdapter ===========================================================
PyROOT::TMemberAdapter::TMemberAdapter( TMethod* meth ) : fMember( meth )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TMethod*() const
{
// cast the adapter to a TMethod* being adapted, returns 0 on failure
   return dynamic_cast< TMethod* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::TMemberAdapter( TFunction* func ) : fMember( func )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TFunction*() const
{
// cast the adapter to a TFunction* being adapted, returns 0 on failure
   return dynamic_cast< TFunction* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::TMemberAdapter( TDataMember* mb ) : fMember( mb )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TDataMember*() const
{
// cast the adapter to a TDataMember* being adapted, returns 0 on failure
   return dynamic_cast< TDataMember* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::TMemberAdapter( TMethodArg* ma ) : fMember( ma )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TMethodArg*() const
{
// cast the adapter to a TMethodArg* being adapted, returns 0 on failure
   return dynamic_cast< TMethodArg* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
std::string PyROOT::TMemberAdapter::Name( unsigned int mod ) const
{
// Return name of the type described by fMember
   TMethodArg* arg = (TMethodArg*)*this;

   if ( arg ) {

      std::string name = arg->GetTypeNormalizedName();
      if ( mod & Rflx::QUALIFIED ) {
      // CLING WORKAROUND -- #100729 get const info from old method
         const std::string& fname = arg->GetFullTypeName();
         if ( fname.substr(0, 5) == "const" )
            return "const " + name;
      // -- CLING WORKAROUND
         return name;
      }

      if ( mod & Rflx::FINAL )
         name = Utility::ResolveTypedef( name );

      return name;

   } else if ( mod & Rflx::FINAL )
      return Utility::ResolveTypedef( fMember->GetName() );

// new with cling, TMethod names are fully scoped ...
   TMethod* m = (TMethod*)*this;
   if (m && m->GetClass()) {
      std::string scoped_name = m->GetName();
      std::string class_name = m->GetClass()->GetName();
      std::string::size_type pos = scoped_name.find(class_name + "::");
      if (pos == 0)      // only accept found at start
         return scoped_name.substr(class_name.size() + 2 /* for :: */, std::string::npos);
   }

// CLING WORKAROUND -- should not be null, but can be due #100389
   if ( fMember != 0 )
      return fMember->GetName();
   return "<unknown>";
}

//____________________________________________________________________________
Bool_t PyROOT::TMemberAdapter::IsEnum() const
{
// test if the adapted member is of an enum type
   return fMember->Property() & kIsEnum;
}

//____________________________________________________________________________
Bool_t PyROOT::TMemberAdapter::IsPublic() const
{
// test if the adapted member represents an public (data) member
   return fMember->Property() & kIsPublic;
}

//____________________________________________________________________________
Bool_t PyROOT::TMemberAdapter::IsStatic() const
{
// test if the adapted member represents a class (data) member
   return fMember->Property() & kIsStatic;
}

//____________________________________________________________________________
size_t PyROOT::TMemberAdapter::FunctionParameterSize( Bool_t required ) const
{
// get the total number of parameters that the adapted function/method takes
   TFunction* func = (TFunction*)fMember;
   if ( ! func )
      return 0;

   if ( required == true )
      return func->GetNargs() - func->GetNargsOpt();

   return func->GetNargs();
}

//____________________________________________________________________________
PyROOT::TMemberAdapter PyROOT::TMemberAdapter::FunctionParameterAt( size_t nth ) const
{
// get the type info of the function parameter at position nth
   return (TMethodArg*)((TFunction*)fMember)->GetListOfMethodArgs()->At( nth );
}

//____________________________________________________________________________
std::string PyROOT::TMemberAdapter::FunctionParameterNameAt( size_t nth ) const
{
// get the formal name, if available, of the function parameter at position nth
   const char* name =
      ((TMethodArg*)((TFunction*)fMember)->GetListOfMethodArgs()->At( nth ))->GetName();

   if ( name )
      return name;
   return "";
}

//____________________________________________________________________________
std::string PyROOT::TMemberAdapter::FunctionParameterDefaultAt( size_t nth ) const
{
// get the default value, if available, of the function parameter at position nth
   TMethodArg* arg = (TMethodArg*)((TFunction*)fMember)->GetListOfMethodArgs()->At( nth );
   const char* def = arg->GetDefault();

   if ( ! def )
      return "";

// special case for strings: "some value" -> ""some value"
   if ( strstr( Utility::ResolveTypedef( arg->GetTypeNormalizedName() ).c_str(), "char*" ) ) {
      std::string sdef = "\"";
      sdef += def;
      sdef += "\"";
      return sdef;
   }

   return def;
}

//____________________________________________________________________________
PyROOT::TReturnTypeAdapter PyROOT::TMemberAdapter::ReturnType() const
{
// get the return type of the wrapped function/method
   return TReturnTypeAdapter( ((TFunction*)fMember)->GetReturnTypeNormalizedName() );
}

//____________________________________________________________________________
PyROOT::TScopeAdapter PyROOT::TMemberAdapter::DeclaringScope() const
{
// get the declaring scope (class) of the wrapped function/method
   TMethod* method = (TMethod*)*this;
   if ( method )
      return method->GetClass();

// happens for free-standing functions (i.e. global scope)
   return std::string( "" );
}

//= TBaseAdapter =============================================================
std::string PyROOT::TBaseAdapter::Name() const
{
// get the name of the base class that is being adapted
   return fBase->GetName();
}

//= TScopeAdapter ============================================================
PyROOT::TScopeAdapter::TScopeAdapter( TClass* klass ) : fClass( klass )
{
// wrap a class (scope)
   if ( fClass.GetClass() != 0 )
      fName = fClass->GetName();
}

//____________________________________________________________________________
PyROOT::TScopeAdapter::TScopeAdapter( const std::string& name ) :
   fClass( name.c_str() ), fName( name )
{
   /* empty */
}

PyROOT::TScopeAdapter::TScopeAdapter( const TMemberAdapter& mb ) :
      fClass( mb.Name( Rflx::SCOPED ).c_str() ),
      fName( mb.Name( Rflx::QUALIFIED | Rflx::SCOPED ) )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TScopeAdapter PyROOT::TScopeAdapter::ByName( const std::string& name, Bool_t quiet )
{
// lookup a scope (class) by name
   Int_t oldEIL = gErrorIgnoreLevel;
   if ( quiet )
      gErrorIgnoreLevel = 3000;
   TClass* klass = TClass::GetClass( name.c_str() );
   gErrorIgnoreLevel = oldEIL;

   return klass;
}

//____________________________________________________________________________
std::string PyROOT::TScopeAdapter::Name( unsigned int mod ) const
{
// Return name of type described by fClass
   if ( ! fClass.GetClass() || ! fClass->Property() ) {
   // fundamental types have no class, and unknown classes have no property
      std::string name = fName;

      if ( ! ( mod & Rflx::QUALIFIED ) )
         name = TClassEdit::CleanType( fName.c_str(), 1 );

      if ( mod & Rflx::FINAL )
         name = Utility::ResolveTypedef( name );

      return name;
   }

   if ( mod & Rflx::FINAL ) {
   /* The following G__ClassInfo lookup is badly broken ...
      G__ClassInfo* clInfo = (G__ClassInfo*)fClass->GetClassInfo();
      if ( mod & Rflx::SCOPED )
         return (clInfo && clInfo->Fullname()) ? clInfo->Fullname() : fClass->GetName();

   // unscoped name ...
      std::string actual = (clInfo && clInfo->Name()) ? clInfo->Name() : fClass->GetName();

   // in case of missing dictionaries, the scope won't have been stripped
      if ( ! ( clInfo && clInfo->IsValid() ) ) {
         std::string::size_type pos = actual.substr( 0, actual.find( '<' ) ).rfind( "::" );
         if ( pos != std::string::npos ) {
         // this is somewhat of a gamble, but the alternative is a guaranteed crash
            actual = actual.substr( pos + 2, std::string::npos );
         }
      }

      return actual;
   */
      return fClass->GetName();

   } else if ( ! ( mod & Rflx::SCOPED ) ) {
   /* The following G__ClassInfo lookup is badly broken ...
      G__ClassInfo* clInfo = (G__ClassInfo*)fClass->GetClassInfo();
      return (clInfo && clInfo->Name()) ? clInfo->Name() : fClass->GetName();
   */
      return fClass->GetName();
   }

   return fClass->GetName();
}

//____________________________________________________________________________
size_t PyROOT::TScopeAdapter::BaseSize() const
{
// get the total number of base classes that this class has
   if ( fClass.GetClass() && fClass->GetListOfBases() != 0 )
      return fClass->GetListOfBases()->GetSize();

   return 0;
}

//____________________________________________________________________________
PyROOT::TBaseAdapter PyROOT::TScopeAdapter::BaseAt( size_t nth ) const
{
// get the nth base of this class
   return (TBaseClass*)fClass->GetListOfBases()->At( nth );
}

//____________________________________________________________________________
size_t PyROOT::TScopeAdapter::FunctionMemberSize() const
{
// get the total number of methods that this class has
   if ( fClass.GetClass() )
      return fClass->GetListOfMethods()->GetSize();

   return 0;
}

//____________________________________________________________________________
PyROOT::TMemberAdapter PyROOT::TScopeAdapter::FunctionMemberAt( size_t nth ) const
{
// get the nth method of this class
   return (TMethod*)fClass->GetListOfMethods()->At( nth );
}

//____________________________________________________________________________
size_t PyROOT::TScopeAdapter::DataMemberSize() const
{
// get the total number of data members that this class has
   if ( fClass.GetClass() )
      return fClass->GetListOfDataMembers()->GetSize();

   return 0;
}

//____________________________________________________________________________
PyROOT::TMemberAdapter PyROOT::TScopeAdapter::DataMemberAt( size_t nth ) const
{
// get the nth data member of this class
   return (TDataMember*)fClass->GetListOfDataMembers()->At( nth );
}

//____________________________________________________________________________
PyROOT::TScopeAdapter::operator Bool_t() const
{
// check the validity of this scope (class)
   if ( fName.empty() )
      return false;

   Bool_t b = kFALSE;

   Int_t oldEIL = gErrorIgnoreLevel;
   gErrorIgnoreLevel = 3000;
   TClass* klass = TClass::GetClass( Name( Rflx::QUALIFIED | Rflx::SCOPED ).c_str() );
   if ( klass && klass->GetClassInfo() )
      b = gInterpreter->ClassInfo_IsValid( klass->GetClassInfo() );
   gErrorIgnoreLevel = oldEIL;
   return b;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsComplete() const
{
// verify whether the dictionary of this class is fully available
   Bool_t b = kFALSE;

   TClass* klass = TClass::GetClass( Name( Rflx::SCOPED ).c_str() );
   if ( klass ) b = gInterpreter->ClassInfo_IsLoaded( klass->GetClassInfo() );
   return b;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsClass() const
{
// test if this scope represents a class
   if ( fClass.GetClass() ) {
   // some inverted logic: we don't have a TClass, but a builtin will be recognized, so
   // if it is NOT a builtin, it is a class or struct (but may be missing dictionary)
      return (fClass->Property() & kIsClass) || ! (fClass->Property() & kIsFundamental);
   }

// no class can mean either is no class (i.e. builtin), or no dict but coming in
// through PyCintex/Reflex ... as a workaround, use TDataTypes that has a full
// enumeration of builtin types
   return TDataType( Name( Rflx::FINAL | Rflx::SCOPED ).c_str() ).GetType() == kOther_t;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsStruct() const
{
// test if this scope represents a struct
   if ( fClass.GetClass() ) {
   // same logic as for IsClass() above ...
      return (fClass->Property() & kIsStruct) || ! (fClass->Property() & kIsFundamental);
   }

// same logic as for IsClass() above ...
   return TDataType( Name( Rflx::FINAL | Rflx::SCOPED ).c_str() ).GetType() == kOther_t;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsNamespace() const
{
// test if this scope represents a namespace
   if ( fClass.GetClass() )
      return fClass->Property() & kIsNamespace;

   return kFALSE;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsAbstract() const
{
// test if this scope represents an abstract class
   if ( fClass.GetClass() )
      return fClass->Property() & kIsAbstract;   // assume set only for classes

   return kFALSE;
}
<commit_msg>remove workaround for #100729<commit_after>// Bindings
#include "PyROOT.h"
#include "Adapters.h"
#include "Utility.h"

// ROOT
#include "TInterpreter.h"
#include "TBaseClass.h"
#include "TClass.h"
#include "TClassEdit.h"
#include "TDataType.h"
#include "TDataMember.h"
#include "TMethod.h"
#include "TFunction.h"
#include "TMethodArg.h"
#include "TList.h"
#include "TError.h"

//= TReturnTypeAdapter =======================================================
std::string PyROOT::TReturnTypeAdapter::Name( unsigned int mod ) const
{
// get the name of the return type that is being adapted
   std::string name = fName;

   if ( ! ( mod & Rflx::QUALIFIED ) )
      name = TClassEdit::CleanType( fName.c_str(), 1 );

   if ( mod & Rflx::FINAL )
      name = Utility::ResolveTypedef( name );

   return name;
}

//= TMemberAdapter ===========================================================
PyROOT::TMemberAdapter::TMemberAdapter( TMethod* meth ) : fMember( meth )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TMethod*() const
{
// cast the adapter to a TMethod* being adapted, returns 0 on failure
   return dynamic_cast< TMethod* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::TMemberAdapter( TFunction* func ) : fMember( func )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TFunction*() const
{
// cast the adapter to a TFunction* being adapted, returns 0 on failure
   return dynamic_cast< TFunction* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::TMemberAdapter( TDataMember* mb ) : fMember( mb )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TDataMember*() const
{
// cast the adapter to a TDataMember* being adapted, returns 0 on failure
   return dynamic_cast< TDataMember* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::TMemberAdapter( TMethodArg* ma ) : fMember( ma )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TMemberAdapter::operator TMethodArg*() const
{
// cast the adapter to a TMethodArg* being adapted, returns 0 on failure
   return dynamic_cast< TMethodArg* >( const_cast< TDictionary* >( fMember ) );
}

//____________________________________________________________________________
std::string PyROOT::TMemberAdapter::Name( unsigned int mod ) const
{
// Return name of the type described by fMember
   TMethodArg* arg = (TMethodArg*)*this;

   if ( arg ) {

      std::string name = arg->GetTypeNormalizedName();
      if ( mod & Rflx::QUALIFIED )
         return name;

      if ( mod & Rflx::FINAL )
         name = Utility::ResolveTypedef( name );

      return name;

   } else if ( mod & Rflx::FINAL )
      return Utility::ResolveTypedef( fMember->GetName() );

// new with cling, TMethod names are fully scoped ...
   TMethod* m = (TMethod*)*this;
   if (m && m->GetClass()) {
      std::string scoped_name = m->GetName();
      std::string class_name = m->GetClass()->GetName();
      std::string::size_type pos = scoped_name.find(class_name + "::");
      if (pos == 0)      // only accept found at start
         return scoped_name.substr(class_name.size() + 2 /* for :: */, std::string::npos);
   }

// CLING WORKAROUND -- should not be null, but can be due #100389
   if ( fMember != 0 )
      return fMember->GetName();
   return "<unknown>";
}

//____________________________________________________________________________
Bool_t PyROOT::TMemberAdapter::IsEnum() const
{
// test if the adapted member is of an enum type
   return fMember->Property() & kIsEnum;
}

//____________________________________________________________________________
Bool_t PyROOT::TMemberAdapter::IsPublic() const
{
// test if the adapted member represents an public (data) member
   return fMember->Property() & kIsPublic;
}

//____________________________________________________________________________
Bool_t PyROOT::TMemberAdapter::IsStatic() const
{
// test if the adapted member represents a class (data) member
   return fMember->Property() & kIsStatic;
}

//____________________________________________________________________________
size_t PyROOT::TMemberAdapter::FunctionParameterSize( Bool_t required ) const
{
// get the total number of parameters that the adapted function/method takes
   TFunction* func = (TFunction*)fMember;
   if ( ! func )
      return 0;

   if ( required == true )
      return func->GetNargs() - func->GetNargsOpt();

   return func->GetNargs();
}

//____________________________________________________________________________
PyROOT::TMemberAdapter PyROOT::TMemberAdapter::FunctionParameterAt( size_t nth ) const
{
// get the type info of the function parameter at position nth
   return (TMethodArg*)((TFunction*)fMember)->GetListOfMethodArgs()->At( nth );
}

//____________________________________________________________________________
std::string PyROOT::TMemberAdapter::FunctionParameterNameAt( size_t nth ) const
{
// get the formal name, if available, of the function parameter at position nth
   const char* name =
      ((TMethodArg*)((TFunction*)fMember)->GetListOfMethodArgs()->At( nth ))->GetName();

   if ( name )
      return name;
   return "";
}

//____________________________________________________________________________
std::string PyROOT::TMemberAdapter::FunctionParameterDefaultAt( size_t nth ) const
{
// get the default value, if available, of the function parameter at position nth
   TMethodArg* arg = (TMethodArg*)((TFunction*)fMember)->GetListOfMethodArgs()->At( nth );
   const char* def = arg->GetDefault();

   if ( ! def )
      return "";

// special case for strings: "some value" -> ""some value"
   if ( strstr( Utility::ResolveTypedef( arg->GetTypeNormalizedName() ).c_str(), "char*" ) ) {
      std::string sdef = "\"";
      sdef += def;
      sdef += "\"";
      return sdef;
   }

   return def;
}

//____________________________________________________________________________
PyROOT::TReturnTypeAdapter PyROOT::TMemberAdapter::ReturnType() const
{
// get the return type of the wrapped function/method
   return TReturnTypeAdapter( ((TFunction*)fMember)->GetReturnTypeNormalizedName() );
}

//____________________________________________________________________________
PyROOT::TScopeAdapter PyROOT::TMemberAdapter::DeclaringScope() const
{
// get the declaring scope (class) of the wrapped function/method
   TMethod* method = (TMethod*)*this;
   if ( method )
      return method->GetClass();

// happens for free-standing functions (i.e. global scope)
   return std::string( "" );
}

//= TBaseAdapter =============================================================
std::string PyROOT::TBaseAdapter::Name() const
{
// get the name of the base class that is being adapted
   return fBase->GetName();
}

//= TScopeAdapter ============================================================
PyROOT::TScopeAdapter::TScopeAdapter( TClass* klass ) : fClass( klass )
{
// wrap a class (scope)
   if ( fClass.GetClass() != 0 )
      fName = fClass->GetName();
}

//____________________________________________________________________________
PyROOT::TScopeAdapter::TScopeAdapter( const std::string& name ) :
   fClass( name.c_str() ), fName( name )
{
   /* empty */
}

PyROOT::TScopeAdapter::TScopeAdapter( const TMemberAdapter& mb ) :
      fClass( mb.Name( Rflx::SCOPED ).c_str() ),
      fName( mb.Name( Rflx::QUALIFIED | Rflx::SCOPED ) )
{
   /* empty */
}

//____________________________________________________________________________
PyROOT::TScopeAdapter PyROOT::TScopeAdapter::ByName( const std::string& name, Bool_t quiet )
{
// lookup a scope (class) by name
   Int_t oldEIL = gErrorIgnoreLevel;
   if ( quiet )
      gErrorIgnoreLevel = 3000;
   TClass* klass = TClass::GetClass( name.c_str() );
   gErrorIgnoreLevel = oldEIL;

   return klass;
}

//____________________________________________________________________________
std::string PyROOT::TScopeAdapter::Name( unsigned int mod ) const
{
// Return name of type described by fClass
   if ( ! fClass.GetClass() || ! fClass->Property() ) {
   // fundamental types have no class, and unknown classes have no property
      std::string name = fName;

      if ( ! ( mod & Rflx::QUALIFIED ) )
         name = TClassEdit::CleanType( fName.c_str(), 1 );

      if ( mod & Rflx::FINAL )
         name = Utility::ResolveTypedef( name );

      return name;
   }

   if ( mod & Rflx::FINAL ) {
   /* The following G__ClassInfo lookup is badly broken ...
      G__ClassInfo* clInfo = (G__ClassInfo*)fClass->GetClassInfo();
      if ( mod & Rflx::SCOPED )
         return (clInfo && clInfo->Fullname()) ? clInfo->Fullname() : fClass->GetName();

   // unscoped name ...
      std::string actual = (clInfo && clInfo->Name()) ? clInfo->Name() : fClass->GetName();

   // in case of missing dictionaries, the scope won't have been stripped
      if ( ! ( clInfo && clInfo->IsValid() ) ) {
         std::string::size_type pos = actual.substr( 0, actual.find( '<' ) ).rfind( "::" );
         if ( pos != std::string::npos ) {
         // this is somewhat of a gamble, but the alternative is a guaranteed crash
            actual = actual.substr( pos + 2, std::string::npos );
         }
      }

      return actual;
   */
      return fClass->GetName();

   } else if ( ! ( mod & Rflx::SCOPED ) ) {
   /* The following G__ClassInfo lookup is badly broken ...
      G__ClassInfo* clInfo = (G__ClassInfo*)fClass->GetClassInfo();
      return (clInfo && clInfo->Name()) ? clInfo->Name() : fClass->GetName();
   */
      return fClass->GetName();
   }

   return fClass->GetName();
}

//____________________________________________________________________________
size_t PyROOT::TScopeAdapter::BaseSize() const
{
// get the total number of base classes that this class has
   if ( fClass.GetClass() && fClass->GetListOfBases() != 0 )
      return fClass->GetListOfBases()->GetSize();

   return 0;
}

//____________________________________________________________________________
PyROOT::TBaseAdapter PyROOT::TScopeAdapter::BaseAt( size_t nth ) const
{
// get the nth base of this class
   return (TBaseClass*)fClass->GetListOfBases()->At( nth );
}

//____________________________________________________________________________
size_t PyROOT::TScopeAdapter::FunctionMemberSize() const
{
// get the total number of methods that this class has
   if ( fClass.GetClass() )
      return fClass->GetListOfMethods()->GetSize();

   return 0;
}

//____________________________________________________________________________
PyROOT::TMemberAdapter PyROOT::TScopeAdapter::FunctionMemberAt( size_t nth ) const
{
// get the nth method of this class
   return (TMethod*)fClass->GetListOfMethods()->At( nth );
}

//____________________________________________________________________________
size_t PyROOT::TScopeAdapter::DataMemberSize() const
{
// get the total number of data members that this class has
   if ( fClass.GetClass() )
      return fClass->GetListOfDataMembers()->GetSize();

   return 0;
}

//____________________________________________________________________________
PyROOT::TMemberAdapter PyROOT::TScopeAdapter::DataMemberAt( size_t nth ) const
{
// get the nth data member of this class
   return (TDataMember*)fClass->GetListOfDataMembers()->At( nth );
}

//____________________________________________________________________________
PyROOT::TScopeAdapter::operator Bool_t() const
{
// check the validity of this scope (class)
   if ( fName.empty() )
      return false;

   Bool_t b = kFALSE;

   Int_t oldEIL = gErrorIgnoreLevel;
   gErrorIgnoreLevel = 3000;
   TClass* klass = TClass::GetClass( Name( Rflx::QUALIFIED | Rflx::SCOPED ).c_str() );
   if ( klass && klass->GetClassInfo() )
      b = gInterpreter->ClassInfo_IsValid( klass->GetClassInfo() );
   gErrorIgnoreLevel = oldEIL;
   return b;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsComplete() const
{
// verify whether the dictionary of this class is fully available
   Bool_t b = kFALSE;

   TClass* klass = TClass::GetClass( Name( Rflx::SCOPED ).c_str() );
   if ( klass ) b = gInterpreter->ClassInfo_IsLoaded( klass->GetClassInfo() );
   return b;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsClass() const
{
// test if this scope represents a class
   if ( fClass.GetClass() ) {
   // some inverted logic: we don't have a TClass, but a builtin will be recognized, so
   // if it is NOT a builtin, it is a class or struct (but may be missing dictionary)
      return (fClass->Property() & kIsClass) || ! (fClass->Property() & kIsFundamental);
   }

// no class can mean either is no class (i.e. builtin), or no dict but coming in
// through PyCintex/Reflex ... as a workaround, use TDataTypes that has a full
// enumeration of builtin types
   return TDataType( Name( Rflx::FINAL | Rflx::SCOPED ).c_str() ).GetType() == kOther_t;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsStruct() const
{
// test if this scope represents a struct
   if ( fClass.GetClass() ) {
   // same logic as for IsClass() above ...
      return (fClass->Property() & kIsStruct) || ! (fClass->Property() & kIsFundamental);
   }

// same logic as for IsClass() above ...
   return TDataType( Name( Rflx::FINAL | Rflx::SCOPED ).c_str() ).GetType() == kOther_t;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsNamespace() const
{
// test if this scope represents a namespace
   if ( fClass.GetClass() )
      return fClass->Property() & kIsNamespace;

   return kFALSE;
}

//____________________________________________________________________________
Bool_t PyROOT::TScopeAdapter::IsAbstract() const
{
// test if this scope represents an abstract class
   if ( fClass.GetClass() )
      return fClass->Property() & kIsAbstract;   // assume set only for classes

   return kFALSE;
}
<|endoftext|>


<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko, Jean-Francois Doyon
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/
//$Id$

//mapnik
#include <mapnik/color.hpp>
// boost
#include <boost/python.hpp>

using mapnik::color;

struct color_pickle_suite : boost::python::pickle_suite
{
    static boost::python::tuple
    getinitargs(const color& c)
    {
        using namespace boost::python;
        return boost::python::make_tuple(c.red(),c.green(),c.blue(),c.alpha());
    }
};

void export_color ()
{
    using namespace boost::python;
    class_<color>("Color", init<int,int,int,int>(
#if BOOST_VERSION >= 103500
        args("self", "r", "g", "b", "a"),
#endif
        "Creates a new color from its RGB components\n"
        "and an alpha value.\n"
        "All values between 0 and 255.\n")
        )
  .def(init<int,int,int>(
#if BOOST_VERSION >= 103500
        args("self", "r", "g", "b"),
#endif
        "Creates a new color from its RGB components.\n"
        "All values between 0 and 255.\n")
        )
  .def(init<std::string>(
#if BOOST_VERSION >= 103500
        args("self", "color_string"),
#endif
        "Creates a new color from its CSS string representation.\n"
        "The string may be a CSS color name (e.g. 'blue')\n"
        "or a hex color string (e.g. '#0000ff').\n")
        )
  .add_property("r",
        &color::red,
        &color::set_red,
        "Gets or sets the red component.\n"
        "The value is between 0 and 255.\n")
  .add_property("g",
        &color::green,
        &color::set_green,
        "Gets or sets the green component.\n"
        "The value is between 0 and 255.\n")
  .add_property("b",
        &color::blue,
        &color::set_blue,
        "Gets or sets the green component.\n"
        "The value is between 0 and 255.\n")
  .add_property("a",
        &color::alpha,
        &color::set_alpha,
        "Gets or sets the green component.\n"
        "The value is between 0 and 255.\n")
  .def(self == self)
  .def_pickle(color_pickle_suite())
  .def("__str__",&color::to_string)
  .def("to_hex_string",&color::to_hex_string,
#if BOOST_VERSION >= 103500
        args("self"),
#endif
        "Returns the hexadecimal representation of this color.\n"
        "\n"
        "Example:\n"
        ">>> c = Color('blue')\n"
        ">>> c.to_hex_string()\n"
        "'#0000ff'\n")
  ;
}

<commit_msg>fix typos in docstrings<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko, Jean-Francois Doyon
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/
//$Id$

//mapnik
#include <mapnik/color.hpp>
// boost
#include <boost/python.hpp>

using mapnik::color;

struct color_pickle_suite : boost::python::pickle_suite
{
    static boost::python::tuple
    getinitargs(const color& c)
    {
        using namespace boost::python;
        return boost::python::make_tuple(c.red(),c.green(),c.blue(),c.alpha());
    }
};

void export_color ()
{
    using namespace boost::python;
    class_<color>("Color", init<int,int,int,int>(
#if BOOST_VERSION >= 103500
        args("self", "r", "g", "b", "a"),
#endif
        "Creates a new color from its RGB components\n"
        "and an alpha value.\n"
        "All values between 0 and 255.\n")
        )
  .def(init<int,int,int>(
#if BOOST_VERSION >= 103500
        args("self", "r", "g", "b"),
#endif
        "Creates a new color from its RGB components.\n"
        "All values between 0 and 255.\n")
        )
  .def(init<std::string>(
#if BOOST_VERSION >= 103500
        args("self", "color_string"),
#endif
        "Creates a new color from its CSS string representation.\n"
        "The string may be a CSS color name (e.g. 'blue')\n"
        "or a hex color string (e.g. '#0000ff').\n")
        )
  .add_property("r",
        &color::red,
        &color::set_red,
        "Gets or sets the red component.\n"
        "The value is between 0 and 255.\n")
  .add_property("g",
        &color::green,
        &color::set_green,
        "Gets or sets the green component.\n"
        "The value is between 0 and 255.\n")
  .add_property("b",
        &color::blue,
        &color::set_blue,
        "Gets or sets the blue component.\n"
        "The value is between 0 and 255.\n")
  .add_property("a",
        &color::alpha,
        &color::set_alpha,
        "Gets or sets the alpha component.\n"
        "The value is between 0 and 255.\n")
  .def(self == self)
  .def_pickle(color_pickle_suite())
  .def("__str__",&color::to_string)
  .def("to_hex_string",&color::to_hex_string,
#if BOOST_VERSION >= 103500
        args("self"),
#endif
        "Returns the hexadecimal representation of this color.\n"
        "\n"
        "Example:\n"
        ">>> c = Color('blue')\n"
        ">>> c.to_hex_string()\n"
        "'#0000ff'\n")
  ;
}

<|endoftext|>


<commit_before>//===-- sanitizer_win_weak_interception.cpp -------------------------------===//
//
// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions.
// See https://llvm.org/LICENSE.txt for license information.
// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
//
//===----------------------------------------------------------------------===//
// This module should be included in the sanitizer when it is implemented as a
// shared library on Windows (dll), in order to delegate the calls of weak
// functions to the implementation in the main executable when a strong
// definition is provided.
//===----------------------------------------------------------------------===//

#include "sanitizer_platform.h"
#if SANITIZER_WINDOWS && SANITIZER_DYNAMIC
#include "sanitizer_win_weak_interception.h"
#include "sanitizer_allocator_interface.h"
#include "sanitizer_interface_internal.h"
#include "sanitizer_win_defs.h"
#include "interception/interception.h"

extern "C" {
void *WINAPI GetModuleHandleA(const char *module_name);
void abort();
}

namespace __sanitizer {
// Try to get a pointer to real_function in the main module and override
// dll_function with that pointer. If the function isn't found, nothing changes.
int interceptWhenPossible(uptr dll_function, const char *real_function) {
  uptr real = __interception::InternalGetProcAddress(
      (void *)GetModuleHandleA(0), real_function);
  if (real && !__interception::OverrideFunction((uptr)dll_function, real, 0))
    abort();
  return 0;
}
} // namespace __sanitizer

// Declare weak hooks.
extern "C" {
void __sanitizer_weak_hook_memcmp(uptr called_pc, const void *s1,
                                  const void *s2, uptr n, int result);
void __sanitizer_weak_hook_strcmp(uptr called_pc, const char *s1,
                                  const char *s2, int result);
void __sanitizer_weak_hook_strncmp(uptr called_pc, const char *s1,
                                   const char *s2, uptr n, int result);
void __sanitizer_weak_hook_strstr(uptr called_pc, const char *s1,
                                  const char *s2, char *result);
}

// Include Sanitizer Common interface.
#define INTERFACE_FUNCTION(Name)
#define INTERFACE_WEAK_FUNCTION(Name) INTERCEPT_SANITIZER_WEAK_FUNCTION(Name)
#include "sanitizer_common_interface.inc"

#pragma section(".WEAK$A", read)
#pragma section(".WEAK$Z", read)

typedef void (*InterceptCB)();
extern "C" {
__declspec(allocate(".WEAK$A")) InterceptCB __start_weak_list;
__declspec(allocate(".WEAK$Z")) InterceptCB __stop_weak_list;
}

static int weak_intercept_init() {
  static bool flag = false;
  // weak_interception_init is expected to be called by only one thread.
  if (flag) return 0;
  flag = true;

  for (InterceptCB *it = &__start_weak_list; it < &__stop_weak_list; ++it)
    if (*it)
      (*it)();

  // In DLLs, the callbacks are expected to return 0,
  // otherwise CRT initialization fails.
  return 0;
}

#pragma section(".CRT$XIB", long, read)
__declspec(allocate(".CRT$XIB")) int (*__weak_intercept_preinit)() =
    weak_intercept_init;

static void WINAPI weak_intercept_thread_init(void *mod, unsigned long reason,
                                              void *reserved) {
  if (reason == /*DLL_PROCESS_ATTACH=*/1) weak_intercept_init();
}

#pragma section(".CRT$XLAB", long, read)
__declspec(allocate(".CRT$XLAB")) void(WINAPI *__weak_intercept_tls_init)(
    void *, unsigned long, void *) = weak_intercept_thread_init;

#endif // SANITIZER_WINDOWS && SANITIZER_DYNAMIC
<commit_msg>[sanitizer_common] Declare __sanitizer_on_print in Windows interception.<commit_after>//===-- sanitizer_win_weak_interception.cpp -------------------------------===//
//
// Part of the LLVM Project, under the Apache License v2.0 with LLVM Exceptions.
// See https://llvm.org/LICENSE.txt for license information.
// SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
//
//===----------------------------------------------------------------------===//
// This module should be included in the sanitizer when it is implemented as a
// shared library on Windows (dll), in order to delegate the calls of weak
// functions to the implementation in the main executable when a strong
// definition is provided.
//===----------------------------------------------------------------------===//

#include "sanitizer_platform.h"
#if SANITIZER_WINDOWS && SANITIZER_DYNAMIC
#include "sanitizer_win_weak_interception.h"
#include "sanitizer_allocator_interface.h"
#include "sanitizer_interface_internal.h"
#include "sanitizer_win_defs.h"
#include "interception/interception.h"

extern "C" {
void *WINAPI GetModuleHandleA(const char *module_name);
void abort();
}

namespace __sanitizer {
// Try to get a pointer to real_function in the main module and override
// dll_function with that pointer. If the function isn't found, nothing changes.
int interceptWhenPossible(uptr dll_function, const char *real_function) {
  uptr real = __interception::InternalGetProcAddress(
      (void *)GetModuleHandleA(0), real_function);
  if (real && !__interception::OverrideFunction((uptr)dll_function, real, 0))
    abort();
  return 0;
}
} // namespace __sanitizer

// Declare weak hooks.
extern "C" {
void __sanitizer_on_print(const char *str);
void __sanitizer_weak_hook_memcmp(uptr called_pc, const void *s1,
                                  const void *s2, uptr n, int result);
void __sanitizer_weak_hook_strcmp(uptr called_pc, const char *s1,
                                  const char *s2, int result);
void __sanitizer_weak_hook_strncmp(uptr called_pc, const char *s1,
                                   const char *s2, uptr n, int result);
void __sanitizer_weak_hook_strstr(uptr called_pc, const char *s1,
                                  const char *s2, char *result);
}

// Include Sanitizer Common interface.
#define INTERFACE_FUNCTION(Name)
#define INTERFACE_WEAK_FUNCTION(Name) INTERCEPT_SANITIZER_WEAK_FUNCTION(Name)
#include "sanitizer_common_interface.inc"

#pragma section(".WEAK$A", read)
#pragma section(".WEAK$Z", read)

typedef void (*InterceptCB)();
extern "C" {
__declspec(allocate(".WEAK$A")) InterceptCB __start_weak_list;
__declspec(allocate(".WEAK$Z")) InterceptCB __stop_weak_list;
}

static int weak_intercept_init() {
  static bool flag = false;
  // weak_interception_init is expected to be called by only one thread.
  if (flag) return 0;
  flag = true;

  for (InterceptCB *it = &__start_weak_list; it < &__stop_weak_list; ++it)
    if (*it)
      (*it)();

  // In DLLs, the callbacks are expected to return 0,
  // otherwise CRT initialization fails.
  return 0;
}

#pragma section(".CRT$XIB", long, read)
__declspec(allocate(".CRT$XIB")) int (*__weak_intercept_preinit)() =
    weak_intercept_init;

static void WINAPI weak_intercept_thread_init(void *mod, unsigned long reason,
                                              void *reserved) {
  if (reason == /*DLL_PROCESS_ATTACH=*/1) weak_intercept_init();
}

#pragma section(".CRT$XLAB", long, read)
__declspec(allocate(".CRT$XLAB")) void(WINAPI *__weak_intercept_tls_init)(
    void *, unsigned long, void *) = weak_intercept_thread_init;

#endif // SANITIZER_WINDOWS && SANITIZER_DYNAMIC
<|endoftext|>


<commit_before>// Copyright eeGeo Ltd 2016, All Rights Reserved

#include "EegeoSearchQuery.h"
#include "IWebLoadRequestFactory.h"
#include "IWebLoadRequest.h"
#include "ApiKey.h"

#include <sstream>
#include <iomanip>

namespace ExampleApp
{
    namespace Search
    {
        namespace EegeoPois
        {
            namespace SdkModel
            {
                EegeoSearchQuery::EegeoSearchQuery(Eegeo::Web::IWebLoadRequestFactory& webRequestFactory,
                                                   Eegeo::Helpers::UrlHelpers::IUrlEncoder& urlEncoder,
                                                   const Search::SdkModel::SearchQuery& query,
                                                   const std::string& apiKey,
                                                   Eegeo::Helpers::ICallback0& completionCallback)
                : m_apiKey(apiKey)
                , m_completionCallback(completionCallback)
                , m_responseString("")
                , m_isSuccess(false)
                , m_webRequestCompleteCallback(this, &EegeoSearchQuery::OnWebResponseReceived)
                {
                    std::string encodedQuery;
                    urlEncoder.UrlEncode(query.Query(), encodedQuery);

                    std::stringstream urlstream;
                    urlstream << "https://poi-staging.eegeo.com";
                    if (query.IsCategory())
                    {
                        urlstream << "/category?c=";
                    }
                    else
                    {
                        urlstream << "/search?s=";
                    }
                    urlstream << encodedQuery;
                    urlstream << "&r=" << std::setprecision(4) << query.Radius();
                    urlstream << "&lat=" << std::setprecision(8) << query.Location().GetLatitudeInDegrees();
                    urlstream << "&lon=" << std::setprecision(8) << query.Location().GetLongitudeInDegrees();
                    urlstream << "&apikey=" << m_apiKey;

                    std::string url = urlstream.str();
                    m_pWebLoadRequest = webRequestFactory.Begin(Eegeo::Web::HttpVerbs::GET, url, m_webRequestCompleteCallback).Build();
                    m_pWebLoadRequest->Load();
                }

                EegeoSearchQuery::~EegeoSearchQuery()
                {

                }

                void EegeoSearchQuery::Cancel()
                {
                    m_pWebLoadRequest->Cancel();
                }

                bool EegeoSearchQuery::IsSucceeded()
                {
                    return m_isSuccess;
                }

                const std::string& EegeoSearchQuery::ResponseString()
                {
                    return m_responseString;
                }

                void EegeoSearchQuery::OnWebResponseReceived(Eegeo::Web::IWebResponse& webResponse)
                {
                    if(webResponse.IsSucceeded())
                    {
                        size_t resultSize = webResponse.GetBodyData().size();
                        m_responseString = std::string(reinterpret_cast<char const*>(&(webResponse.GetBodyData().front())), resultSize);
                        m_isSuccess = true;
                    }

                    m_completionCallback();
                }
            }
        }
    }
}
<commit_msg>Point to production poi instance and use versioned API<commit_after>// Copyright eeGeo Ltd 2016, All Rights Reserved

#include "EegeoSearchQuery.h"
#include "IWebLoadRequestFactory.h"
#include "IWebLoadRequest.h"
#include "ApiKey.h"

#include <sstream>
#include <iomanip>

namespace ExampleApp
{
    namespace Search
    {
        namespace EegeoPois
        {
            namespace SdkModel
            {
                EegeoSearchQuery::EegeoSearchQuery(Eegeo::Web::IWebLoadRequestFactory& webRequestFactory,
                                                   Eegeo::Helpers::UrlHelpers::IUrlEncoder& urlEncoder,
                                                   const Search::SdkModel::SearchQuery& query,
                                                   const std::string& apiKey,
                                                   Eegeo::Helpers::ICallback0& completionCallback)
                : m_apiKey(apiKey)
                , m_completionCallback(completionCallback)
                , m_responseString("")
                , m_isSuccess(false)
                , m_webRequestCompleteCallback(this, &EegeoSearchQuery::OnWebResponseReceived)
                {
                    std::string encodedQuery;
                    urlEncoder.UrlEncode(query.Query(), encodedQuery);

                    std::stringstream urlstream;
                    urlstream << "https://poi.eegeo.com/v1";
                    if (query.IsCategory())
                    {
                        urlstream << "/category?c=";
                    }
                    else
                    {
                        urlstream << "/search?s=";
                    }
                    urlstream << encodedQuery;
                    urlstream << "&r=" << std::setprecision(4) << query.Radius();
                    urlstream << "&lat=" << std::setprecision(8) << query.Location().GetLatitudeInDegrees();
                    urlstream << "&lon=" << std::setprecision(8) << query.Location().GetLongitudeInDegrees();
                    urlstream << "&apikey=" << m_apiKey;

                    std::string url = urlstream.str();
                    m_pWebLoadRequest = webRequestFactory.Begin(Eegeo::Web::HttpVerbs::GET, url, m_webRequestCompleteCallback).Build();
                    m_pWebLoadRequest->Load();
                }

                EegeoSearchQuery::~EegeoSearchQuery()
                {

                }

                void EegeoSearchQuery::Cancel()
                {
                    m_pWebLoadRequest->Cancel();
                }

                bool EegeoSearchQuery::IsSucceeded()
                {
                    return m_isSuccess;
                }

                const std::string& EegeoSearchQuery::ResponseString()
                {
                    return m_responseString;
                }

                void EegeoSearchQuery::OnWebResponseReceived(Eegeo::Web::IWebResponse& webResponse)
                {
                    if(webResponse.IsSucceeded())
                    {
                        size_t resultSize = webResponse.GetBodyData().size();
                        m_responseString = std::string(reinterpret_cast<char const*>(&(webResponse.GetBodyData().front())), resultSize);
                        m_isSuccess = true;
                    }

                    m_completionCallback();
                }
            }
        }
    }
}
<|endoftext|>


<commit_before>#include <zlib.h>
#include <lcm/lcm-cpp.hpp>

#include <lcmtypes/bot_core/planar_lidar_t.hpp>
#include <lcmtypes/bot_core/pointcloud_t.hpp>
#include <lcmtypes/bot_core/pose_t.hpp>
#include <lcmtypes/bot_core/rigid_transform_t.hpp>

#include <boost/shared_ptr.hpp>
#include "lidar-odometry.hpp"
#include <ConciseArgs>

#include <bot_param/param_client.h>
#include <bot_frames/bot_frames.h>

using namespace std;

struct CommandLineConfig
{
  bool use_velodyne;
  bool init_with_message; // initialize off of a pose or vicon
  std::string output_channel;
};

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_);

    ~App(){
    }

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    const CommandLineConfig cl_cfg_;

    BotParam* botparam_;
    BotFrames* botframes_;

    void lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);
    void pointCloudHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pointcloud_t* msg);
    LidarOdom* lidarOdom_;

    Eigen::Isometry3d body_to_lidar_; // Fixed tf from the lidar to the robot's base link
    Eigen::Isometry3d world_to_body_init_; // Captures the position of the body frame in world at launch
    Eigen::Isometry3d world_to_body_now_; // running position estimate

    // Init handlers:
    void rigidTransformInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg);
    void poseInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg);
    void initState( const double trans[3], const double quat[4]);
    bool pose_initialized_;

    bot_core::pose_t getPoseAsBotPose(Eigen::Isometry3d pose, int64_t utime);

    int get_trans_with_utime(BotFrames *bot_frames,
        const char *from_frame, const char *to_frame, int64_t utime,
        Eigen::Isometry3d& mat);
};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_) :
       lcm_(lcm_), cl_cfg_(cl_cfg_){

  // Set up frames and config:
  botparam_ = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0);
  botframes_= bot_frames_get_global(lcm_->getUnderlyingLCM(), botparam_);

  if (cl_cfg_.use_velodyne){
    lcm_->subscribe("VELODYNE_HORIZONTAL",&App::pointCloudHandler,this);
    int status = get_trans_with_utime( botframes_ ,  "VELODYNE", "body"  , 0, body_to_lidar_);
  }else{
    lcm_->subscribe("SCAN",&App::lidarHandler,this);
    int status = get_trans_with_utime( botframes_ ,  "SCAN", "body"  , 0, body_to_lidar_);
  }

  pose_initialized_ = false;
  if (!cl_cfg_.init_with_message){
    std::cout << "Init internal est using default\n";
    world_to_body_init_ = Eigen::Isometry3d::Identity();

    /*
    world_to_body_init_.setIdentity();
    world_to_body_init_.translation()  << 1.2, 1.67, 0.32;
    Eigen::Quaterniond q(-0.045668, -0.004891, -0.00909, 0.9989);
    world_to_body_init_.rotate(q);
    bot_core::pose_t pose_msg_body = getPoseAsBotPose( world_to_lidar_init_ , 0);
    lcm_->publish("POSE_BODY_ALT", &pose_msg_body );
    pose_initialized_ = true;
    */

    pose_initialized_ = true;
  }else{
    lcm_->subscribe("VICON_BODY|VICON_FRONTPLATE",&App::rigidTransformInitHandler,this);
    lcm_->subscribe("POSE_VICON",&App::poseInitHandler,this);
    std::cout << "Waiting for Init message to LIDAR estimator\n";
  }

  lidarOdom_ = new LidarOdom(lcm_);
}

int App::get_trans_with_utime(BotFrames *bot_frames,
        const char *from_frame, const char *to_frame, int64_t utime,
        Eigen::Isometry3d & mat){
  int status;
  double matx[16];
  status = bot_frames_get_trans_mat_4x4_with_utime( bot_frames, from_frame,  to_frame, utime, matx);
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      mat(i,j) = matx[i*4+j];
    }
  }

  return status;
}

bot_core::pose_t App::getPoseAsBotPose(Eigen::Isometry3d pose, int64_t utime){
  bot_core::pose_t pose_msg;
  pose_msg.utime =   utime;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  Eigen::Quaterniond r_x(pose.rotation());
  pose_msg.orientation[0] =  r_x.w();
  pose_msg.orientation[1] =  r_x.x();
  pose_msg.orientation[2] =  r_x.y();
  pose_msg.orientation[3] =  r_x.z();
  return pose_msg;
}

void App::lidarHandler(const lcm::ReceiveBuffer* rbuf,
     const std::string& channel, const  bot_core::planar_lidar_t* msg){
  if (!pose_initialized_){
    std::cout << "Estimate not initialised, exiting lidarHandler\n";
    return;
  }

  // 1. Update LIDAR Odometry
  std::vector<float> ranges_copy = msg->ranges;
  float* ranges = &ranges_copy[0];
  lidarOdom_->doOdometry(ranges, msg->nranges, msg->rad0, msg->radstep, msg->utime);

  // 2. Determine the body position using the LIDAR motion estimate:
  Eigen::Isometry3d lidar_init_to_lidar_now = lidarOdom_->getCurrentPose();
  Eigen::Isometry3d world_to_lidar_now = world_to_body_init_*body_to_lidar_*lidar_init_to_lidar_now;
  world_to_body_now_ = world_to_lidar_now * body_to_lidar_.inverse();

  //bot_core::pose_t pose_msg = getPoseAsBotPose( lidar_init_to_lidar_now , msg->utime);
  //lcm_->publish("POSE_BODY_ALT", &pose_msg );
  bot_core::pose_t pose_msg_body = getPoseAsBotPose( world_to_body_now_ , msg->utime);
  lcm_->publish(cl_cfg_.output_channel, &pose_msg_body );
}

void App::pointCloudHandler(const lcm::ReceiveBuffer* rbuf,
     const std::string& channel, const  bot_core::pointcloud_t* msg){
  if (!pose_initialized_){
    std::cout << "Estimate not initialised, exiting pointCloudHandler\n";
    return;
  }

  // 1. Update LIDAR Odometry
  std::vector<float> x;
  std::vector<float> y;
  for (int i=0;i<msg->n_points; i++){
    x.push_back( msg->points[i][0] );
    y.push_back( msg->points[i][1] );
  }

  lidarOdom_->doOdometry(x, y, msg->n_points, msg->utime);

  // 2. Determine the body position using the LIDAR motion estimate:
  Eigen::Isometry3d lidar_init_to_lidar_now = lidarOdom_->getCurrentPose();
  Eigen::Isometry3d world_to_lidar_now = world_to_body_init_*body_to_lidar_*lidar_init_to_lidar_now;
  world_to_body_now_ = world_to_lidar_now * body_to_lidar_.inverse();

  //bot_core::pose_t pose_msg = getPoseAsBotPose( world_to_lidar_now , msg->utime);
  //lcm_->publish("POSE_BODY_ALT", &pose_msg );
  bot_core::pose_t pose_msg_body = getPoseAsBotPose( world_to_body_now_ , msg->utime);
  lcm_->publish(cl_cfg_.output_channel, &pose_msg_body );
}

void App::initState(const double trans[3], const double quat[4]){
  if ( !cl_cfg_.init_with_message || pose_initialized_ ){
    return;
  }

  std::cout << "Init internal est using rigid transform or pose\n";

  world_to_body_init_.setIdentity();
  world_to_body_init_.translation()  << trans[0], trans[1] , trans[2];
  Eigen::Quaterniond quatE = Eigen::Quaterniond(quat[0], quat[1],
                                               quat[2], quat[3]);
  world_to_body_init_.rotate(quatE);
  pose_initialized_ = TRUE;
}

void App::rigidTransformInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg){
  initState(msg->trans, msg->quat);
}

void App::poseInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg){
  initState(msg->pos, msg->orientation);
}

int main(int argc, char **argv){
  CommandLineConfig cl_cfg;
  cl_cfg.use_velodyne = false;
  cl_cfg.init_with_message = TRUE;
  cl_cfg.output_channel = "POSE_BODY";

  ConciseArgs parser(argc, argv, "simple-fusion");
  parser.add(cl_cfg.init_with_message, "g", "init_with_message", "Bootstrap internal estimate using VICON or POSE_INIT");
  parser.add(cl_cfg.output_channel, "o", "output_channel", "Output message e.g POSE_BODY");
  parser.add(cl_cfg.use_velodyne, "v", "use_velodyne", "Use a velodyne instead of the LIDAR");
  parser.parse();

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }
  App app= App(lcm, cl_cfg);
  while(0 == lcm->handle());
}
<commit_msg>Introduced init channel as argument<commit_after>#include <zlib.h>
#include <lcm/lcm-cpp.hpp>

#include <lcmtypes/bot_core/planar_lidar_t.hpp>
#include <lcmtypes/bot_core/pointcloud_t.hpp>
#include <lcmtypes/bot_core/pose_t.hpp>
#include <lcmtypes/bot_core/rigid_transform_t.hpp>

#include <boost/shared_ptr.hpp>
#include "lidar-odometry.hpp"
#include <ConciseArgs>

#include <bot_param/param_client.h>
#include <bot_frames/bot_frames.h>

using namespace std;

struct CommandLineConfig
{
  bool use_velodyne;
  bool init_with_message; // initialize off of a pose or vicon
  std::string output_channel;
  std::string init_channel;
};

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_);

    ~App(){
    }

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    const CommandLineConfig cl_cfg_;

    BotParam* botparam_;
    BotFrames* botframes_;

    void lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);
    void pointCloudHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pointcloud_t* msg);
    LidarOdom* lidarOdom_;

    Eigen::Isometry3d body_to_lidar_; // Fixed tf from the lidar to the robot's base link
    Eigen::Isometry3d world_to_body_init_; // Captures the position of the body frame in world at launch
    Eigen::Isometry3d world_to_body_now_; // running position estimate

    // Init handlers:
    void rigidTransformInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg);
    void poseInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg);
    void initState( const double trans[3], const double quat[4]);
    bool pose_initialized_;

    bot_core::pose_t getPoseAsBotPose(Eigen::Isometry3d pose, int64_t utime);

    int get_trans_with_utime(BotFrames *bot_frames,
        const char *from_frame, const char *to_frame, int64_t utime,
        Eigen::Isometry3d& mat);
};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_) :
       lcm_(lcm_), cl_cfg_(cl_cfg_){

  // Set up frames and config:
  botparam_ = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0);
  botframes_= bot_frames_get_global(lcm_->getUnderlyingLCM(), botparam_);

  if (cl_cfg_.use_velodyne){
    lcm_->subscribe("VELODYNE_HORIZONTAL",&App::pointCloudHandler,this);
    int status = get_trans_with_utime( botframes_ ,  "VELODYNE", "body"  , 0, body_to_lidar_);
  }else{
    lcm_->subscribe("SCAN",&App::lidarHandler,this);
    int status = get_trans_with_utime( botframes_ ,  "SCAN", "body"  , 0, body_to_lidar_);
  }

  pose_initialized_ = false;
  if (!cl_cfg_.init_with_message){
    std::cout << "Init internal est using default\n";
    world_to_body_init_ = Eigen::Isometry3d::Identity();

    /*
    world_to_body_init_.setIdentity();
    world_to_body_init_.translation()  << 1.2, 1.67, 0.32;
    Eigen::Quaterniond q(-0.045668, -0.004891, -0.00909, 0.9989);
    world_to_body_init_.rotate(q);
    bot_core::pose_t pose_msg_body = getPoseAsBotPose( world_to_lidar_init_ , 0);
    lcm_->publish("POSE_BODY_ALT", &pose_msg_body );
    pose_initialized_ = true;
    */

    pose_initialized_ = true;
  }else{
    lcm_->subscribe("VICON_BODY|VICON_FRONTPLATE",&App::rigidTransformInitHandler,this);
    lcm_->subscribe(cl_cfg_.init_channel,&App::poseInitHandler,this);
    std::cout << "Waiting for Init message to LIDAR estimator\n";
  }

  lidarOdom_ = new LidarOdom(lcm_);
}

int App::get_trans_with_utime(BotFrames *bot_frames,
        const char *from_frame, const char *to_frame, int64_t utime,
        Eigen::Isometry3d & mat){
  int status;
  double matx[16];
  status = bot_frames_get_trans_mat_4x4_with_utime( bot_frames, from_frame,  to_frame, utime, matx);
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      mat(i,j) = matx[i*4+j];
    }
  }

  return status;
}

bot_core::pose_t App::getPoseAsBotPose(Eigen::Isometry3d pose, int64_t utime){
  bot_core::pose_t pose_msg;
  pose_msg.utime =   utime;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  Eigen::Quaterniond r_x(pose.rotation());
  pose_msg.orientation[0] =  r_x.w();
  pose_msg.orientation[1] =  r_x.x();
  pose_msg.orientation[2] =  r_x.y();
  pose_msg.orientation[3] =  r_x.z();
  return pose_msg;
}

void App::lidarHandler(const lcm::ReceiveBuffer* rbuf,
     const std::string& channel, const  bot_core::planar_lidar_t* msg){
  if (!pose_initialized_){
    std::cout << "Estimate not initialised, exiting lidarHandler\n";
    return;
  }

  // 1. Update LIDAR Odometry
  std::vector<float> ranges_copy = msg->ranges;
  float* ranges = &ranges_copy[0];
  lidarOdom_->doOdometry(ranges, msg->nranges, msg->rad0, msg->radstep, msg->utime);

  // 2. Determine the body position using the LIDAR motion estimate:
  Eigen::Isometry3d lidar_init_to_lidar_now = lidarOdom_->getCurrentPose();
  Eigen::Isometry3d world_to_lidar_now = world_to_body_init_*body_to_lidar_*lidar_init_to_lidar_now;
  world_to_body_now_ = world_to_lidar_now * body_to_lidar_.inverse();

  //bot_core::pose_t pose_msg = getPoseAsBotPose( lidar_init_to_lidar_now , msg->utime);
  //lcm_->publish("POSE_BODY_ALT", &pose_msg );
  bot_core::pose_t pose_msg_body = getPoseAsBotPose( world_to_body_now_ , msg->utime);
  lcm_->publish(cl_cfg_.output_channel, &pose_msg_body );
}

void App::pointCloudHandler(const lcm::ReceiveBuffer* rbuf,
     const std::string& channel, const  bot_core::pointcloud_t* msg){
  if (!pose_initialized_){
    std::cout << "Estimate not initialised, exiting pointCloudHandler\n";
    return;
  }

  // 1. Update LIDAR Odometry
  std::vector<float> x;
  std::vector<float> y;
  for (int i=0;i<msg->n_points; i++){
    x.push_back( msg->points[i][0] );
    y.push_back( msg->points[i][1] );
  }

  lidarOdom_->doOdometry(x, y, msg->n_points, msg->utime);

  // 2. Determine the body position using the LIDAR motion estimate:
  Eigen::Isometry3d lidar_init_to_lidar_now = lidarOdom_->getCurrentPose();
  Eigen::Isometry3d world_to_lidar_now = world_to_body_init_*body_to_lidar_*lidar_init_to_lidar_now;
  world_to_body_now_ = world_to_lidar_now * body_to_lidar_.inverse();

  //bot_core::pose_t pose_msg = getPoseAsBotPose( world_to_lidar_now , msg->utime);
  //lcm_->publish("POSE_BODY_ALT", &pose_msg );
  bot_core::pose_t pose_msg_body = getPoseAsBotPose( world_to_body_now_ , msg->utime);
  lcm_->publish(cl_cfg_.output_channel, &pose_msg_body );
}

void App::initState(const double trans[3], const double quat[4]){
  if ( !cl_cfg_.init_with_message || pose_initialized_ ){
    return;
  }

  std::cout << "Init internal est using rigid transform or pose\n";

  world_to_body_init_.setIdentity();
  world_to_body_init_.translation()  << trans[0], trans[1] , trans[2];
  Eigen::Quaterniond quatE = Eigen::Quaterniond(quat[0], quat[1],
                                               quat[2], quat[3]);
  world_to_body_init_.rotate(quatE);
  pose_initialized_ = TRUE;
}

void App::rigidTransformInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::rigid_transform_t* msg){
  initState(msg->trans, msg->quat);
}

void App::poseInitHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg){
  initState(msg->pos, msg->orientation);
}

int main(int argc, char **argv){
  CommandLineConfig cl_cfg;
  cl_cfg.use_velodyne = false;
  cl_cfg.init_with_message = TRUE;
  cl_cfg.output_channel = "POSE_BODY";

  ConciseArgs parser(argc, argv, "simple-fusion");
  parser.add(cl_cfg.init_with_message, "g", "init_with_message", "Bootstrap internal estimate using VICON or POSE_INIT");
  parser.add(cl_cfg.output_channel, "o", "output_channel", "Output message e.g POSE_BODY");
  parser.add(cl_cfg.use_velodyne, "v", "use_velodyne", "Use a velodyne instead of the LIDAR");
  parser.add(cl_cfg.init_channel, "i", "init_channel", "Read the init message from this channel, e.g., POSE_GROUND_TRUTH");
  parser.parse();

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }
  App app= App(lcm, cl_cfg);
  while(0 == lcm->handle());
}
<|endoftext|>


<commit_before>#include <cp-neural.h>

// Manual build:
// g++ -g -ggdb -I ../cpneural -I /usr/local/include/eigen3 minitest.cpp -L ../Build/cpneural/ -lcpneural -lpthread -o test

using std::cerr; using std::endl;

bool matComp(MatrixN& m0, MatrixN& m1, string msg="", floatN eps=1.e-6) {
    if (m0.cols() != m1.cols() || m0.rows() != m1.rows()) {
        cerr << msg << ": Incompatible shapes " << shape(m0) << "!=" << shape(m1) << endl;
        return false;
    }
    MatrixN d = m0 - m1;
    floatN dif = d.cwiseProduct(d).sum();
    if (dif < eps) {
        cerr << msg << " err=" << dif << endl;
        return true;
    } else {
        IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
        cerr << msg << " m0:" << endl << m0.format(CleanFmt) << endl;
        cerr << msg << " m1:" << endl << m1.format(CleanFmt) << endl;
        cerr << msg << "  ∂:" << endl << (m0-m1).format(CleanFmt) << endl;
        cerr << "err=" << dif << endl;
        return false;
    }
}

int doTests() {
    MatrixN w(10,10);
    MatrixN wx;
    cerr << "-------------Standard-init" << endl;
    wx=xavierInit(w,XavierMode::XAV_STANDARD);
    cerr << wx << endl;
    cerr << "-------------Normal-init" << endl;
    wx=xavierInit(w,XavierMode::XAV_NORMAL);
    cerr << wx << endl;
    cerr << "-------------Orthonormal-init" << endl;
    wx=xavierInit(w,XavierMode::XAV_ORTHONORMAL);
    cerr << wx << endl;

    cerr << "-------------Orthonormal-init (check multi)" << endl;
    MatrixN o=wx * wx.transpose();
    cerr << o << endl;

    return 0;
}

int tFunc(floatN x, int c) {
    int y=(int)(((sin(x*2.0)+1.0)/2.0)*(floatN)c);
    //cerr << x << ":" << y << " " << endl;
    return y;
}

bool trainTest(string init) {
    bool allOk=true;
    json j;
    int N=300,NV=30,NT=30,I=5,H=20,C=4;
    j["inputShape"]=vector<int>{I};
    j["hidden"]=vector<int>{H,C};
    j["init"]=init;
    TwoLayerNet tln(j);

    MatrixN X(N,I);
    X.setRandom();
    MatrixN y(N,1);
    for (unsigned i=0; i<y.rows(); i++) y(i,0)=tFunc(X(i,0),C);

    MatrixN Xv(NV,I);
    Xv.setRandom();
    MatrixN yv(NV,1);
    for (unsigned i=0; i<yv.rows(); i++) yv(i,0)=tFunc(Xv(i,0),C);

    MatrixN Xt(NT,I);
    Xt.setRandom();
    MatrixN yt(NT,1);
    for (unsigned i=0; i<yt.rows(); i++) yt(i,0)=tFunc(Xt(i,0),C);

    json jo(R"({"verbose":false,"epochs":100.0,"batch_size":20,"learning_rate":1e-2,"lr_decay":1.0,"momentum":0.9,"decay_rate":0.98,"epsilon":1e-8,"threads":2})"_json);

    floatN train_err,test_err,val_err;

    tln.train(X, y, Xv, yv, "Adam", jo);
    //tln.train(X, y, Xv, yv, "Sdg", cpo);
    train_err=tln.test(X, y);
    val_err=tln.test(Xv, yv);
    test_err=tln.test(Xt, yt);

    cerr << "Train-test, train-err=" << train_err << endl;
    cerr << "       validation-err=" << val_err << endl;
    cerr << "       final test-err=" << val_err << endl;
    if (test_err>0.2 || val_err>0.2 || train_err>0.2) allOk=false;
    return allOk;
}

int main(int argc, char *argv[]) {
    string name="test";
    cpInitCompute(name);
    int ret=0;
    ret=0;
    //doTests();

    cerr << "standard init=============" << endl;
    trainTest("standard");
    cerr << "normal init===============" << endl;
    trainTest("normal");
    cerr << "orthonormal init==========" << endl;
    trainTest("orthonormal");
    cerr << "orthogonal init==========" << endl;
    trainTest("orthogonal");
    cpExitCompute();

    json j;
    j["test"]=vector<int>{3,4,5};
    j["turbo"]["traffic"]["tangente"]=3.13;
    cerr << j << endl;
    if (j["tandem"]["kat"]==nullptr)
        cerr << "not defined"<< endl;
    string a;
    a=j.value("murksel","tada");
    cerr << a << endl;
    j["murksel"]="ding";
    a=j.value("murksel","tada");
    cerr << a << endl;
    int b;
    b=j.value("hidden",13);
    cerr << b << endl;
    j["hidden"]=1024;
    b=j.value("hidden",13);
    cerr << b << endl;

    json j2 = R"({"test":32,"turbo":"loader","c2": [2]})"_json;
    cerr << j2 << endl;
    string l=j2["turbo"];
    cerr << j2["turbo"]<<endl;
    cerr << j.dump(4) << endl;
    cerr << j2.dump(4) << endl;
    return ret;

}
<commit_msg>Start with h5-tests'<commit_after>#include <cp-neural.h>

// Manual build:
// g++ -g -ggdb -I ../cpneural -I /usr/local/include/eigen3 minitest.cpp -L ../Build/cpneural/ -lcpneural -lpthread -o test

using std::cerr; using std::endl;

bool matComp(MatrixN& m0, MatrixN& m1, string msg="", floatN eps=1.e-6) {
    if (m0.cols() != m1.cols() || m0.rows() != m1.rows()) {
        cerr << msg << ": Incompatible shapes " << shape(m0) << "!=" << shape(m1) << endl;
        return false;
    }
    MatrixN d = m0 - m1;
    floatN dif = d.cwiseProduct(d).sum();
    if (dif < eps) {
        cerr << msg << " err=" << dif << endl;
        return true;
    } else {
        IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
        cerr << msg << " m0:" << endl << m0.format(CleanFmt) << endl;
        cerr << msg << " m1:" << endl << m1.format(CleanFmt) << endl;
        cerr << msg << "  ∂:" << endl << (m0-m1).format(CleanFmt) << endl;
        cerr << "err=" << dif << endl;
        return false;
    }
}

int doTests() {
    MatrixN w(10,10);
    MatrixN wx;
    cerr << "-------------Standard-init" << endl;
    wx=xavierInit(w,XavierMode::XAV_STANDARD);
    cerr << wx << endl;
    cerr << "-------------Normal-init" << endl;
    wx=xavierInit(w,XavierMode::XAV_NORMAL);
    cerr << wx << endl;
    cerr << "-------------Orthonormal-init" << endl;
    wx=xavierInit(w,XavierMode::XAV_ORTHONORMAL);
    cerr << wx << endl;

    cerr << "-------------Orthonormal-init (check multi)" << endl;
    MatrixN o=wx * wx.transpose();
    cerr << o << endl;

    return 0;
}

int tFunc(floatN x, int c) {
    int y=(int)(((sin(x*2.0)+1.0)/2.0)*(floatN)c);
    //cerr << x << ":" << y << " " << endl;
    return y;
}

bool trainTest(string init) {
    bool allOk=true;
    json j;
    int N=300,NV=30,NT=30,I=5,H=20,C=4;
    j["inputShape"]=vector<int>{I};
    j["hidden"]=vector<int>{H,C};
    j["init"]=init;
    TwoLayerNet tln(j);

    MatrixN X(N,I);
    X.setRandom();
    MatrixN y(N,1);
    for (unsigned i=0; i<y.rows(); i++) y(i,0)=tFunc(X(i,0),C);

    MatrixN Xv(NV,I);
    Xv.setRandom();
    MatrixN yv(NV,1);
    for (unsigned i=0; i<yv.rows(); i++) yv(i,0)=tFunc(Xv(i,0),C);

    MatrixN Xt(NT,I);
    Xt.setRandom();
    MatrixN yt(NT,1);
    for (unsigned i=0; i<yt.rows(); i++) yt(i,0)=tFunc(Xt(i,0),C);

    json jo(R"({"verbose":false,"epochs":100.0,"batch_size":20,"learning_rate":1e-2,"lr_decay":1.0,"momentum":0.9,"decay_rate":0.98,"epsilon":1e-8,"threads":2})"_json);

    floatN train_err,test_err,val_err;

    tln.train(X, y, Xv, yv, "Adam", jo);
    //tln.train(X, y, Xv, yv, "Sdg", cpo);
    train_err=tln.test(X, y);
    val_err=tln.test(Xv, yv);
    test_err=tln.test(Xt, yt);

    cerr << "Train-test, train-err=" << train_err << endl;
    cerr << "       validation-err=" << val_err << endl;
    cerr << "       final test-err=" << val_err << endl;
    if (test_err>0.2 || val_err>0.2 || train_err>0.2) allOk=false;
    return allOk;
}

void initTest() {
    cerr << "standard init=============" << endl;
    trainTest("standard");
    cerr << "normal init===============" << endl;
    trainTest("normal");
    cerr << "orthonormal init==========" << endl;
    trainTest("orthonormal");
    cerr << "orthogonal init==========" << endl;
    trainTest("orthogonal");
}

void jsonTest() {
    json j;
    j["test"]=vector<int>{3,4,5};
    j["turbo"]["traffic"]["tangente"]=3.13;
    cerr << j << endl;
    if (j["tandem"]["kat"]==nullptr)
        cerr << "not defined"<< endl;
    string a;
    a=j.value("murksel","tada");
    cerr << a << endl;
    j["murksel"]="ding";
    a=j.value("murksel","tada");
    cerr << a << endl;
    int b;
    b=j.value("hidden",13);
    cerr << b << endl;
    j["hidden"]=1024;
    b=j.value("hidden",13);
    cerr << b << endl;

    json j2 = R"({"test":32,"turbo":"loader","c2": [2]})"_json;
    cerr << j2 << endl;
    string l=j2["turbo"];
    cerr << j2["turbo"]<<endl;
    cerr << j.dump(4) << endl;
    cerr << j2.dump(4) << endl;
}

void hd5Test() {
    int inputShapeFlat=1024;
    int hidden=1024;
    string inittype{"standard"};
    floatN initfactor=1.0;

    json j1;
    j1["inputShape"]=vector<int>{inputShapeFlat};
    j1["hidden"]=hidden;
    j1["init"]=inittype;
    j1["initfactor"]=initfactor;

    string filepath{"minitest.h5"};
    H5::H5File file((H5std_string)filepath, H5F_ACC_TRUNC );
    // H5File file(FILE_NAME, H5F_ACC_RDWR);

    Affine *af=new Affine(j1);
    af->saveParameters(&file);
    file.close();

    H5::H5File fmn((H5std_string)filepath, H5F_ACC_RDONLY);

}

int main(int argc, char *argv[]) {
    string name="test";
    cpInitCompute(name);
    int ret=0;

    hd5Test();

    cpExitCompute();
    return ret;

}
<|endoftext|>