simple_dgm.hpp

Go to the documentation of this file.

#include "dynamic_graph_manager/TestUserCmdBool.h"
#include "dynamic_graph_manager/dynamic_graph_manager.hh"
#include "dynamic_graph_manager/ros_init.hh"

namespace dynamic_graph_manager
{
class SimpleDGM : public dynamic_graph::DynamicGraphManager
{
public:
    SimpleDGM() : dynamic_graph::DynamicGraphManager()
    {
        boolean_set_by_user_cmd_ = false;
    }
    ~SimpleDGM()
    {
    }
    void initialize_hardware_communication_process()
    {
        // get the hardware communication ros node handle
        ros::NodeHandle& ros_node_handle = dynamic_graph::ros_init(
            dynamic_graph::DynamicGraphManager::hw_com_ros_node_name_);
        ros_user_commands_.push_back(ros_node_handle.advertiseService(
            "set_a_boolean", &SimpleDGM::user_command_callback, this));
    }
    void get_sensors_to_map(dynamic_graph::VectorDGMap& map)
    {
        map["encoders"].setRandom();
        map["imu_accelerometer"].setRandom();
        map["imu_gyroscope"].setRandom();
        map["imu"].setRandom();
    }
    void set_motor_controls_from_map(const dynamic_graph::VectorDGMap& map)
    {
        desired_torques_ = map.at("torques");
        desired_positions_ = map.at("positions");
    }

    bool get_has_user_command_been_executed()
    {
        // Here we nee to protect the access to this ressource as it may
        // conflict with different thread. Please use cond_var_ to make sure
        // everything is access on its due time.
        bool ret = boolean_set_by_user_cmd_;
        return ret;
    }

    bool user_command_callback(
        dynamic_graph_manager::TestUserCmdBool::Request& req,
        dynamic_graph_manager::TestUserCmdBool::Response& res)
    {
        // parse and register the command for further call.
        add_user_command(
            std::bind(&SimpleDGM::user_command, this, req.input_boolean));
        // return whatever the user want
        res.sanity_check = true;

        // the service has been executed properly
        return true;
    }

    bool is_in_safety_mode()
    {
        // here define a check for robot specific stuff.
        bool check = true;
        return check || DynamicGraphManager::is_in_safety_mode();
    }

    void compute_safety_controls()
    {
        // fill all controls with zero... Check your robot to imagine what would
        // be safer in this case.
        for (dynamic_graph::VectorDGMap::iterator ctrl =
                 motor_controls_map_.begin();
             ctrl != motor_controls_map_.end();
             ++ctrl)
        {
            ctrl->second.fill(0.0);
        }
    }

private:
    void user_command(bool user_input)
    {
        // do something with the internal state or with the hardware
        boolean_set_by_user_cmd_ = user_input;
    }
    // some internal hardware class or obect. Here just a simple boolean for
    // unit testing
    std::atomic_bool boolean_set_by_user_cmd_;

    // Control
    dynamicgraph::Vector desired_torques_, desired_positions_;
};

}  // namespace dynamic_graph_manager