romeo.h

Go to the documentation of this file.

#ifndef ROMEO_DCM_DRIVER_ROMEO_H
#define ROMEO_DCM_DRIVER_ROMEO_H

// Boost Headers
#include <boost/shared_ptr.hpp>
#include <boost/assign.hpp>
#include <boost/lexical_cast.hpp>

// NAOqi Headers
#include <alcommon/almodule.h>
#include <alcommon/alproxy.h>
#include <alproxies/almotionproxy.h>
#include <alproxies/almemoryproxy.h>
#include <alproxies/dcmproxy.h>
#include <qi/os.hpp>

// ROS Headers
#include <ros/ros.h>

/*
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
*/
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>

#include <romeo_dcm_msgs/BoolService.h>
/*#include <romeo_dcm_msgs/FSRs.h>
#include <romeo_dcm_msgs/Bumper.h>
#include <romeo_dcm_msgs/Tactile.h> */
#include <std_msgs/Float32.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

#include <controller_manager/controller_manager.h>

#include <diagnostic_updater/diagnostic_updater.h>

using std::string;
using std::vector;

namespace AL
{
class ALBroker;
}

// Helper definition
template<typename T, size_t N>
T * end(T (&ra)[N]) {
    return ra + N;
}

class Romeo : public AL::ALModule, public hardware_interface::RobotHW
{
private:
    // ROS Standard Variables
    ros::NodeHandle node_handle_;

    // ROS Topics/Messages
    ros::Subscriber cmd_vel_sub_;

    ros::Publisher stiffness_pub_;
    ros::Publisher m_jointStatePub;

    //Stiffness and joint states
    std_msgs::Float32 stiffness_;
    sensor_msgs::JointState m_jointState;
    ros::ServiceServer stiffness_switch_;

    controller_manager::ControllerManager* manager_;

    // ROS Diagnostics
    diagnostic_updater::Updater diagnostic_;

    // Member Variables
    AL::ALValue commands_;

    // Helper
    bool is_connected_;

    // Robot Parameters
    string version_, body_type_;
    bool stiffnesses_enabled_;
    int topic_queue_;
    string prefix_, odom_frame_;
    double low_freq_, high_freq_, controller_freq_, joint_precision_;

    // AL Proxies
    AL::ALMemoryProxy memory_proxy_;
    boost::shared_ptr<AL::ALProxy> dcm_proxy_;//To control the motion
    boost::shared_ptr<AL::ALMotionProxy> m_motionProxy;//To get the joints' states

    // Joints
    vector<string> joints_names_;

    // Joint States
    hardware_interface::JointStateInterface jnt_state_interface_;
    hardware_interface::PositionJointInterface jnt_pos_interface_;

    int number_of_joints_;
    vector<string> joint_names_;
    vector<double> joint_commands_;
    vector<double> joint_angles_;
    vector<double> joint_velocities_;
    vector<double> joint_efforts_;
public:
    // Constructor/Destructor
    Romeo(boost::shared_ptr<AL::ALBroker> broker, const std::string& name);
    ~Romeo();

    bool initialize();
    bool initializeControllers(controller_manager::ControllerManager& cm);

    // Connect/Disconnet to ALProxies
    bool connect(const ros::NodeHandle nh);
    void disconnect();

    // Subscribe/Advertise to ROS Topics/Services
    void subscribe();

    // Parameter Server
    void loadParams();

    // Helper
    void brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier);

    // DCMProxy Wrapper Methods
    void DCMTimedCommand(const string& key, const AL::ALValue& value, const int& timeOffset,
                         const string& type="Merge");
    void DCMAliasTimedCommand(const string& alias, const vector<float>& values, const vector<int>& timeOffsets,
                              const string& type="Merge", const string& type2="time-mixed");

    // ALMemoryProxy Wrapper Methods
    void insertDataToMemory(const string& key, const AL::ALValue& value);
    AL::ALValue getDataFromMemory(const string& key);
    void subscribeToEvent(const std::string& name, const std::string& callback_module,
                          const std::string& callback_method);
    void subscribeToMicroEvent(const std::string& name, const std::string& callback_module,
                               const std::string& callback_method, const string& callback_message="");
    void unsubscribeFromEvent(const string& name, const string& callback_module);
    void unsubscribeFromMicroEvent(const string& name, const string& callback_module);
    void raiseEvent(const string& name, const AL::ALValue& value);
    void raiseMicroEvent(const string& name, const AL::ALValue& value);
    void declareEvent(const string& name);

    // General Methods
    void controllerLoop();
    void lowCommunicationLoop();
    void highCommunicationLoop();

    bool connected();

    // ROS Callbacks/Related Methods
    void commandVelocity(const geometry_msgs::TwistConstPtr &msg);
    bool switchStiffnesses(romeo_dcm_msgs::BoolService::Request &req, romeo_dcm_msgs::BoolService::Response &res);
    void readJoints();
    void publishJointStateFromAlMotion();

    void writeJoints();
    void run();

};

#endif // ROMEO_H