Ejemplo n.º 1
0
    bool updateModule()
    {
        double verg = 0;

        Bottle bin;
        Bottle bout;
        inport.read(bin);
        if (reset_offset)
        {
            initial_offset[0] = bin.get(0).asDouble();
            initial_offset[1] = bin.get(1).asDouble();
            initial_offset[2] = bin.get(2).asDouble();
            initial_offset[3] = bin.get(3).asDouble();
            initial_offset[4] = bin.get(4).asDouble();
            initial_offset[5] = bin.get(5).asDouble();
            reset_offset = false;
        }

        double x_torque = fabs(bin.get(3).asDouble()) - initial_offset [3];
        double y_force = fabs(bin.get(1).asDouble()) - initial_offset[1];
        bout.addInt(2);
        double j_torque = x_torque + y_force * 0.70; //Nm + N * m
        bout.addDouble(j_torque); //j0
        bout.addDouble(j_torque); //j1
        bout.addDouble(j_torque); //j2
        bout.addDouble(j_torque); //j3
        bout.addDouble(j_torque); //j4
        bout.addDouble(j_torque); //j5
        outport.write(bout);

        return true;
    }
Ejemplo n.º 2
0
    /**
    * Terminates module execution and performs cleanup
    * @return this method cannot fail, so it always returns true
    */
    virtual bool close()
    {
        if (m_ptf.isValid())
        {
            m_ptf.close();
        }
        if (m_pmap.isValid())
        {
            m_pmap.close();
        }

        if (m_rosPublisher_occupancyGrid.asPort().isOpen())
        {
            m_rosPublisher_occupancyGrid.interrupt();
            m_rosPublisher_occupancyGrid.close();
        }
        if (m_rosPublisher_initial_pose.asPort().isOpen())
        {
            m_rosPublisher_initial_pose.interrupt();
            m_rosPublisher_initial_pose.close();
        }
        if (m_rosNode)
        {
            delete m_rosNode;
            m_rosNode = 0;
        }
        m_rpcPort.interrupt();
        m_rpcPort.close();

        return true;
    }
Ejemplo n.º 3
0
    virtual bool close()
    {
        rpcPort.interrupt();
        rpcPort.close();

        //gotoThread->shutdown();
        gotoThread->stop();
        delete gotoThread;
        gotoThread=NULL;

        return true;
    }
Ejemplo n.º 4
0
    virtual bool close()
    {
        mHandlerPort.interrupt();
        mHandlerPort.close();

        //mNavThread->shutdown();
        mNavThread->stop();
        delete mNavThread;
        mNavThread=NULL;

        return true;
    }
Ejemplo n.º 5
0
 virtual bool threadInit() override
 {
     p.open("/readWriteTest_writer");
     image.resize(100, 100);
     image.zero();
     return true;
 }
Ejemplo n.º 6
0
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();
        
        mNavThread=new Navigator(&rf);

        if (!mNavThread->start())
        {
            delete mNavThread;
            return false;
        }

        std::string local=rf.check("local",yarp::os::Value("/ikartnav")).asString().c_str();
        mHandlerPort.open(local.c_str());
        attach(mHandlerPort);
        //attachTerminal();

        return true;
    }
Ejemplo n.º 7
0
 bool configure( yarp::os::ResourceFinder &rf )
   {
       std::string moduleName = rf.check("name", 
               yarp::os::Value("demoServerModule"), 
               "module name (string)").asString().c_str();
       setName(moduleName.c_str());
       
       std::string slash="/";
       
       attach(cmdPort);
       
       std::string cmdPortName= "/";
       cmdPortName+= getName();
       cmdPortName += "/cmd";
       if (!cmdPort.open(cmdPortName.c_str())) {           
           std::cout << getName() << ": Unable to open port " << cmdPortName << std::endl;  
           return false;
       }
       return true;
   }   
Ejemplo n.º 8
0
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();

        Property p;
        ConstString configFile = rf.findFile("from");
        if (configFile!="") p.fromConfigFile(configFile.c_str());

        gotoThread = new GotoThread(10,rf,p);

        if (!gotoThread->start())
        {
            delete gotoThread;
            return false;
        }

        rpcPort.open("/ikartGoto/rpc:i");
        attach(rpcPort);
        //attachTerminal();

        return true;
    }
Ejemplo n.º 9
0
    /**
    * Interrupts ports operations
    * @return this method cannot fail, so it always returns true
    */
    virtual bool interruptModule()
    {
        m_rpcPort.interrupt();

        return true;
    }
Ejemplo n.º 10
0
    /**
    * Performs module configuration, parsing user options stored in the resource finder.
    * Available options are described in main module documentation.
    * @return true if the module was successfully configured and opened, false otherwise.
    */
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();

        m_rpcPort.open("/"+m_module_name+"/rpc");
        attach(m_rpcPort);
        //attachTerminal();
        m_rf = rf;
        
        //configuration file checking
        Bottle general_group = m_rf.findGroup("GENERAL");
        if (general_group.isNull())
        {
            yError() << "Missing GENERAL group!";
            return false;
        }

        Bottle initial_group = m_rf.findGroup("INITIAL_POS");
        if (initial_group.isNull())
        {
            yError() << "Missing INITIAL_POS group!";
            return false;
        }

        Bottle localization_group = m_rf.findGroup("LOCALIZATION");
        if (localization_group.isNull())
        {
            yError() << "Missing LOCALIZATION group!";
            return false;
        }

        Bottle ros_group = m_rf.findGroup("ROS");

        Bottle tf_group = m_rf.findGroup("TF");
        if (tf_group.isNull())
        {
            yError() << "Missing TF group!";
            return false;
        }

        Bottle odometry_group = m_rf.findGroup("ODOMETRY");
        if (odometry_group.isNull())
        {
            yError() << "Missing ODOMETRY group!";
            return false;
        }

        Bottle map_group = m_rf.findGroup("MAP");
        if (map_group.isNull())
        {
            yError() << "Missing MAP group!";
            return false;
        }
        yDebug() << map_group.toString();

        //general group
        if (general_group.check("module_name") == false)
        {
            yError() << "Missing `module_name` in [GENERAL] group";
            return false;
        }
        m_module_name = general_group.find("module_name").asString();

        if (general_group.check("enable_ros") == false)
        {
            yError() << "Missing `ros_enable` in [GENERAL] group";
            return false;
        }
        m_ros_enabled = (general_group.find("enable_ros").asInt()==1);

        //ROS group
        if (m_ros_enabled)
        {
            m_rosNode = new yarp::os::Node("/"+m_module_name);
            
            //initialize an occupancy grid publisher (every time the localization is re-initialized, the map is published too)
            if (ros_group.check ("occupancygrid_topic"))
            {
                m_topic_occupancyGrid = ros_group.find ("occupancygrid_topic").asString();
                if (!m_rosPublisher_occupancyGrid.topic(m_topic_occupancyGrid))
                {
                     if (m_rosNode)
                     {
                         delete m_rosNode;
                         m_rosNode=0;
                     }
                     yError() << "localizationModule: unable to publish data on " << m_topic_occupancyGrid << " topic, check your yarp-ROS network configuration";
                     return false;
                }
            }

             //initialize an initial pose publisher
            if (ros_group.check ("initialpose_topic")) 
            {
                m_topic_initial_pose = ros_group.find ("initialpose_topic").asString();
                {
                    if (!m_rosPublisher_initial_pose.topic(m_topic_initial_pose))
                    {
                        if (m_rosNode)
                        {
                            delete m_rosNode;
                            m_rosNode=0;
                        }
                        yError() << "localizationModule: unable to publish data on " << m_topic_initial_pose << " topic, check your yarp-ROS network configuration";
                        return false;
                    }
                }
            }
        }

        //localization group
        if (localization_group.check("use_localization_from_odometry_port")) { m_use_localization_from_odometry_port = (localization_group.find("use_localization_from_odometry_port").asInt() == 1); }
        if (localization_group.check("use_localization_from_tf"))   { m_use_localization_from_tf = (localization_group.find("use_localization_from_tf").asInt() == 1); }

        if (m_use_localization_from_odometry_port == true && m_use_localization_from_tf == true)
        {
            yError() << "`use_localization_from_tf` and `use_localization_from_odometry_port` cannot be true simultaneously!";
            return false;
        }

        //map server group
        yDebug() << map_group.toString();

        if (map_group.check("connect_to_yarp_mapserver") == false)
        {
            yError() << "Missing `connect_to_yarp_mapserver` in [MAP] group";
            return false;
        }
        m_use_map_server= (map_group.find("connect_to_yarp_mapserver").asInt()==1);

        //tf group
        if (tf_group.check("map_frame_id") == false)
        {
            yError() << "Missing `map_frame_id` in [TF] group";
            return false;
        }
        if (tf_group.check("robot_frame_id") == false)
        {
            yError() << "Missing `robot_frame_id` in [TF] group";
            return false;
        }
        m_frame_map_id = tf_group.find("map_frame_id").asString();
        m_frame_robot_id = tf_group.find("robot_frame_id").asString();

        //odometry group
        if (odometry_group.check("odometry_broadcast_port") == false)
        {
            yError() << "Missing `odometry_port` in [ODOMETRY] group";
            return false;
        }
        m_port_broadcast_odometry_name = odometry_group.find("odometry_broadcast_port").asString();

        //device driver opening and/or connections
        if (m_use_localization_from_odometry_port)
        {
            //opens a YARP port to receive odometry data
            std::string odom_portname = "/" + m_module_name + "/odometry:i";
            bool b1 = m_port_odometry_input.open(odom_portname.c_str());
            bool b2 = yarp::os::Network::sync(odom_portname.c_str(),false);
            bool b3 = yarp::os::Network::connect(m_port_broadcast_odometry_name.c_str(), odom_portname.c_str());
            if (b1 == false || b2 ==false || b3==false)
            {
                yError() << "Unable to initialize odometry port connection from " << m_port_broadcast_odometry_name.c_str()<< "to:" << odom_portname.c_str();
                return false;
            }
        }

        if (m_use_localization_from_tf)
        {
            //opens a client to receive localization data from transformServer
            Property options;
            options.put("device", "transformClient");
            options.put("local", "/"+m_module_name + "/TfClient");
            options.put("remote", "/transformServer");
            if (m_ptf.open(options) == false)
            {
                yError() << "Unable to open transform client";
                return false;
            }
            m_ptf.view(m_iTf);
            if (m_ptf.isValid() == false || m_iTf == 0)
            {
                yError() << "Unable to view iTransform interface";
                return false;
            }
        }

        if (m_use_map_server)
        {
            //opens a client to send/received data from mapServer
            Property map_options;
            map_options.put("device", "map2DClient");
            map_options.put("local", "/" +m_module_name); //This is just a prefix. map2DClient will complete the port name.
            map_options.put("remote", "/mapServer");
            if (m_pmap.open(map_options) == false)
            {
                yWarning() << "Unable to open mapClient";
            }
            else
            {
                yInfo() << "Opened mapClient";
                m_pmap.view(m_iMap);
                if (m_pmap.isValid() == false || m_iMap == 0)
                {
                    yError() << "Unable to view map interface";
                    return false;
                }
            }
        }

        //initial location initialization
        if (initial_group.check("initial_x"))     { m_initial_loc.x = initial_group.find("initial_x").asDouble(); }
        else { yError() << "missing initial_x param"; return false; }
        if (initial_group.check("initial_y"))     { m_initial_loc.y = initial_group.find("initial_y").asDouble(); }
        else { yError() << "missing initial_y param"; return false; }
        if (initial_group.check("initial_theta")) { m_initial_loc.theta = initial_group.find("initial_theta").asDouble(); }
        else { yError() << "missing initial_theta param"; return false; }
        if (initial_group.check("initial_map"))   { m_initial_loc.map_id = initial_group.find("initial_map").asString(); }
        else { yError() << "missing initial_map param"; return false; }
        this->initializeLocalization(m_initial_loc);

        return true;
    }
Ejemplo n.º 11
0
 bool close()
 {
     cmdPort.close(); 
     return true;
 }
Ejemplo n.º 12
0
 bool attach(yarp::os::Port& source)
 {
     attachedToPort=true;
     source.setReader(*this);
     return true;
 }
Ejemplo n.º 13
0
 virtual void run() override
 {
     p.write(image);
     yarp::os::Time::delay(1);
 }
Ejemplo n.º 14
0
 virtual void threadRelease() override
 {
     p.close();
 }
Ejemplo n.º 15
0
 virtual bool close()
 {
     inport.close();
     outport.close();
     return true;
 }
Ejemplo n.º 16
0
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        Property options;
        options.fromString(rf.toString());
        char robotName[255];
        Bottle *jointsList=0;
        std::string moduleName = "torqueObserver";
        Time::turboBoost();

        reset_offset = true;
        for (int i=0; i<6; i++) initial_offset[i]=0.0;

       /* options.put("device", "remote_controlboard");
        if(options.check("robot"))
            strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
        else
            strncpy(robotName, "icub", sizeof(robotName));

        if (options.check("name"))
        {
            moduleName = options.find("name").asString();
        }

        if(options.check("part"))
        {
            sprintf(partName, "%s", options.find("part").asString().c_str());

            sprintf(tmp, "/%s/%s/%s/client", moduleName.c_str(), robotName, partName);
            options.put("local",tmp);
        
            sprintf(tmp, "/%s/%s", robotName, partName);
            options.put("remote", tmp);
        
            sprintf(tmp, "/%s/%s/rpc", moduleName.c_str(), partName);
            rpc_port.open(tmp);
            
            options.put("carrier", "tcp");
            
            attach(rpc_port);
        }
        else
        {
            yError("Please specify part (e.g. --part head)\n");
            return false;
        }

        //opening the device driver
        if (!driver.open(options))
        {
            yError("Error opening device, check parameters\n");
            return false;
        }

        bool ret = true;
        idir = 0;
        icmd = 0;
        ret &= driver.view(idir);
        ret &= driver.view(icmd);
      */
        char pin[255];
        sprintf(pin, "/%s/torque:i", moduleName.c_str());
        inport.open(pin);
        char pout[255];
        sprintf(pout, "/%s/torque:o", moduleName.c_str());
        outport.open(pout);

        bool b1 = yarp::os::Network::connect("/icub/left_leg/analog:o",pin);
        bool b2 = yarp::os::Network::connect(pout,"/icub/left_leg/analog:o");
        if (!b1)
        {
            yWarning("failed input connection");
        }
        if (!b2)
        {
            yWarning("failed ouput connection");
        }
        return true;
    }
Ejemplo n.º 17
0
    virtual bool interruptModule()
    {
        mHandlerPort.interrupt();

        return true;
    }