sitting_demo.h

// Motion demonstration on a sitting robot
// Authors: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>
//          Sebastian Schüller <schuell1@cs.uni-bonn.de>

// Ensure header is only included once
#ifndef SITTING_DEMO_H
#define SITTING_DEMO_H

// Includes
#include <nimbro_op_interface/LEDCommand.h>
#include <sitting_demo/sitting_demo_fsm.h>
#include <limb_control/PlayCommandsSrv.h>
#include <nimbro_op_interface/Button.h>
#include <config_server/parameter.h>
#include <demo_msgs/DemoFSMState.h>
#include <robotcontrol/State.h>
#include <ros/service.h>
#include <ros/time.h>

// Sitting demo namespace
namespace sitting_demo
{
    // SittingDemo class
    class SittingDemo
    {
    public:
        // Constructor/destructor
        SittingDemo();

        // Initialisation
        void loadDemoMotions();

        // Main loop step
        bool step();
        bool halt();

        // Motion functions
        void playMotion(int motionID);
        double sendLimbCommands(const limb_control::PlayCommandsRequest& PCmdReq);

        // Utility functions
        void publishFSMState(const demo_msgs::DemoFSMState& state);
        void publishAction(const demo_msgs::DemoFSMState& action);

        // Get functions
        inline int numMotions() const { return (int) m_demoMotions.size(); }
        inline bool haveMotions() const { return m_haveMotions; }
        inline bool enabled() const { return m_enabled && m_sittingDemoEnabled(); }
        inline bool robotIsSitting() const { return m_robotIsSitting; }
        inline const ros::Time& curStepTime() const { return m_curStepTime; }
        inline const ros::Time& limbControlFinishTime() const { return m_limbControlFinishTime; }

    private:
        // Demo state controller
        SittingDemoSC m_sc;

        // ROS topics
        ros::Publisher m_pub_FSMState;
        ros::Publisher m_pub_action;
        ros::Publisher m_pub_LEDCommand;

        // Robot LEDs
        void publishLEDCommand();
        nimbro_op_interface::LEDCommand m_LED;

        // ROS services
        ros::ServiceClient m_srv_playMotion;
        ros::ServiceClient m_srv_playCommands;

        // Robotcontrol robot state
        ros::Subscriber m_sub_robotState;
        void handleRobotState(const robotcontrol::StateConstPtr& msg);
        std::string m_robotStateName;
        bool m_robotIsSitting;

        // Button presses
        ros::Subscriber m_sub_button;
        void handleButton(const nimbro_op_interface::ButtonConstPtr& msg);
        bool m_buttonPressed;
        bool m_enabled;

        // Configuration parameters
        config_server::Parameter<bool> m_sittingDemoEnabled;
        void handleDemoEnabled();

        // Motions
        std::vector<std::string> m_demoMotions;
        bool m_haveMotions;

        // Internal variables
        ros::Time m_curStepTime;
        ros::Time m_limbControlFinishTime;
    };
}

#endif /* SITTING_DEMO_H */
// EOF