romeo.h
Go to the documentation of this file.
00001 
00023 #ifndef ROMEO_DCM_DRIVER_ROMEO_H
00024 #define ROMEO_DCM_DRIVER_ROMEO_H
00025 
00026 // Boost Headers
00027 #include <boost/shared_ptr.hpp>
00028 #include <boost/assign.hpp>
00029 #include <boost/lexical_cast.hpp>
00030 
00031 // NAOqi Headers
00032 #include <alcommon/almodule.h>
00033 #include <alcommon/alproxy.h>
00034 #include <alproxies/almotionproxy.h>
00035 #include <alproxies/almemoryproxy.h>
00036 #include <alproxies/dcmproxy.h>
00037 #include <qi/os.hpp>
00038 
00039 // ROS Headers
00040 #include <ros/ros.h>
00041 
00042 /*
00043 #include <geometry_msgs/Twist.h>
00044 #include <sensor_msgs/Imu.h>
00045 #include <sensor_msgs/Range.h>
00046 */
00047 #include <nav_msgs/Odometry.h>
00048 #include <sensor_msgs/JointState.h>
00049 
00050 #include <romeo_dcm_msgs/BoolService.h>
00051 /*#include <romeo_dcm_msgs/FSRs.h>
00052 #include <romeo_dcm_msgs/Bumper.h>
00053 #include <romeo_dcm_msgs/Tactile.h> */
00054 #include <std_msgs/Float32.h>
00055 
00056 #include <tf/transform_broadcaster.h>
00057 #include <tf/transform_listener.h>
00058 #include <tf/transform_datatypes.h>
00059 
00060 #include <hardware_interface/joint_command_interface.h>
00061 #include <hardware_interface/joint_state_interface.h>
00062 #include <hardware_interface/robot_hw.h>
00063 
00064 #include <controller_manager/controller_manager.h>
00065 
00066 #include <diagnostic_updater/diagnostic_updater.h>
00067 
00068 using std::string;
00069 using std::vector;
00070 
00071 namespace AL
00072 {
00073 class ALBroker;
00074 }
00075 
00076 // Helper definition
00077 template<typename T, size_t N>
00078 T * end(T (&ra)[N]) {
00079     return ra + N;
00080 }
00081 
00082 class Romeo : public AL::ALModule, public hardware_interface::RobotHW
00083 {
00084 private:
00085     // ROS Standard Variables
00086     ros::NodeHandle node_handle_;
00087 
00088     // ROS Topics/Messages
00089     ros::Subscriber cmd_vel_sub_;
00090 
00091     ros::Publisher stiffness_pub_;
00092     ros::Publisher m_jointStatePub;
00093 
00094     //Stiffness and joint states
00095     std_msgs::Float32 stiffness_;
00096     sensor_msgs::JointState m_jointState;
00097     ros::ServiceServer stiffness_switch_;
00098 
00099     controller_manager::ControllerManager* manager_;
00100 
00101     // ROS Diagnostics
00102     diagnostic_updater::Updater diagnostic_;
00103 
00104     // Member Variables
00105     AL::ALValue commands_;
00106 
00107     // Helper
00108     bool is_connected_;
00109 
00110     // Robot Parameters
00111     string version_, body_type_;
00112     bool stiffnesses_enabled_;
00113     int topic_queue_;
00114     string prefix_, odom_frame_;
00115     double low_freq_, high_freq_, controller_freq_, joint_precision_;
00116 
00117     // AL Proxies
00118     AL::ALMemoryProxy memory_proxy_;
00119     boost::shared_ptr<AL::ALProxy> dcm_proxy_;//To control the motion
00120     boost::shared_ptr<AL::ALMotionProxy> m_motionProxy;//To get the joints' states
00121 
00122     // Joints
00123     vector<string> joints_names_;
00124 
00125     // Joint States
00126     hardware_interface::JointStateInterface jnt_state_interface_;
00127     hardware_interface::PositionJointInterface jnt_pos_interface_;
00128 
00129     int number_of_joints_;
00130     vector<string> joint_names_;
00131     vector<double> joint_commands_;
00132     vector<double> joint_angles_;
00133     vector<double> joint_velocities_;
00134     vector<double> joint_efforts_;
00135 public:
00136     // Constructor/Destructor
00137     Romeo(boost::shared_ptr<AL::ALBroker> broker, const std::string& name);
00138     ~Romeo();
00139 
00140     bool initialize();
00141     bool initializeControllers(controller_manager::ControllerManager& cm);
00142 
00143     // Connect/Disconnet to ALProxies
00144     bool connect(const ros::NodeHandle nh);
00145     void disconnect();
00146 
00147     // Subscribe/Advertise to ROS Topics/Services
00148     void subscribe();
00149 
00150     // Parameter Server
00151     void loadParams();
00152 
00153     // Helper
00154     void brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier);
00155 
00156     // DCMProxy Wrapper Methods
00157     void DCMTimedCommand(const string& key, const AL::ALValue& value, const int& timeOffset,
00158                          const string& type="Merge");
00159     void DCMAliasTimedCommand(const string& alias, const vector<float>& values, const vector<int>& timeOffsets,
00160                               const string& type="Merge", const string& type2="time-mixed");
00161 
00162     // ALMemoryProxy Wrapper Methods
00163     void insertDataToMemory(const string& key, const AL::ALValue& value);
00164     AL::ALValue getDataFromMemory(const string& key);
00165     void subscribeToEvent(const std::string& name, const std::string& callback_module,
00166                           const std::string& callback_method);
00167     void subscribeToMicroEvent(const std::string& name, const std::string& callback_module,
00168                                const std::string& callback_method, const string& callback_message="");
00169     void unsubscribeFromEvent(const string& name, const string& callback_module);
00170     void unsubscribeFromMicroEvent(const string& name, const string& callback_module);
00171     void raiseEvent(const string& name, const AL::ALValue& value);
00172     void raiseMicroEvent(const string& name, const AL::ALValue& value);
00173     void declareEvent(const string& name);
00174 
00175     // General Methods
00176     void controllerLoop();
00177     void lowCommunicationLoop();
00178     void highCommunicationLoop();
00179 
00180     bool connected();
00181 
00182     // ROS Callbacks/Related Methods
00183     void commandVelocity(const geometry_msgs::TwistConstPtr &msg);
00184     bool switchStiffnesses(romeo_dcm_msgs::BoolService::Request &req, romeo_dcm_msgs::BoolService::Response &res);
00185     void readJoints();
00186     void publishJointStateFromAlMotion();
00187 
00188     void writeJoints();
00189     void run();
00190 
00191 };
00192 
00193 #endif // ROMEO_H


romeo_dcm_driver
Author(s): Konstantinos Chatzilygeroudis , Ha Dang
autogenerated on Fri Jun 24 2016 04:21:15