arm_navigation_controls.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 * \brief RVIZ plugin which provides simplified GUI based on Warehouse Viewer.
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Zdenek Materna (imaterna@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: 5/4/2012
 * 
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef BUT_ARMNAVIGATION_H
#define BUT_ARMNAVIGATION_H

#include <wx/wx.h>
#include <wx/menu.h>
#include <wx/panel.h>
#include <wx/dialog.h>
#include <wx/msgdlg.h>
#include <wx/sizer.h>
#include <wx/tglbtn.h>
#include <wx/checkbox.h>
#include <wx/choice.h>

#include <ros/ros.h>
#include <string.h>

#include "srs_assisted_arm_navigation_msgs/ArmNavExecute.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavNew.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavPlan.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavPlay.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavReset.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavStart.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavSuccess.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavFailed.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavRefresh.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavSwitchAttCO.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavRepeat.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavStep.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavStop.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavMovePalmLinkRel.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavMovePalmLink.h"
#include "srs_assisted_arm_navigation_msgs/AssistedArmNavigationState.h"

//#include "srs_env_model/LockCollisionMap.h"

//#include "cob_script_server/ScriptAction.h"
#include <actionlib/client/simple_action_client.h>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <map>

#include "srs_assisted_arm_navigation/services_list.h"
#include "srs_env_model/services_list.h"
#include <tf/transform_broadcaster.h>
#include <wx/textctrl.h>

namespace rviz
{
    class WindowManagerInterface;
}

namespace srs_assisted_arm_navigation_ui {

static const bool WAIT_FOR_START = true;

//typedef actionlib::SimpleActionClient<cob_script_server::ScriptAction> cob_client;


struct Preset {

        std::string name;
        geometry_msgs::Pose pose;
        bool relative;

};

class CArmManipulationControls : public wxPanel
{
public:
    CArmManipulationControls(wxWindow *parent, const wxString& title, rviz::WindowManagerInterface * wmi );
    ~CArmManipulationControls();

    virtual void OnNew(wxCommandEvent& event);
    virtual void OnPlan(wxCommandEvent& event);

    virtual void OnExecute(wxCommandEvent& event);

    //virtual void OnFinish(wxCommandEvent& event);
    virtual void OnSuccess(wxCommandEvent& event);
    virtual void OnFailed(wxCommandEvent& event);
    virtual void OnRepeat(wxCommandEvent& event);

    virtual void OnSwitch(wxCommandEvent& event);



    void OnStepBack(wxCommandEvent& event);

    void OnChoice(wxCommandEvent& event);

    void MoveAbs();
    void MoveRel();

    void OnMoveRel(wxCommandEvent& event);

    void OnGrasp(wxCommandEvent& event);
    void OnStopGrasp(wxCommandEvent& event);

    void OnLift(wxCommandEvent& event);
    void OnHold(wxCommandEvent& event);

    void OnStopTraj(wxCommandEvent& event);

    void OnRoll(wxCommandEvent& event);
    void OnPitch(wxCommandEvent& event);
    void OnYaw(wxCommandEvent& event);

    void setControlsToDefaultState();
    void disableControls();

    bool refresh();


    void OnSetText(wxCommandEvent & event);

    bool nav_start(srs_assisted_arm_navigation_msgs::ArmNavStart::Request &req, srs_assisted_arm_navigation_msgs::ArmNavStart::Response &res);

protected:
    rviz::WindowManagerInterface * m_wmi;

    typedef std::map<std::string, wxButton *> ButtonsMap;

    ButtonsMap buttons_;

    /*wxSlider * m_slider_roll;
    wxSlider * m_slider_pitch;
    wxSlider * m_slider_yaw;*/


    wxChoice *presets_choice_;

    wxButton * m_button_switch;

    wxStaticText *m_text_status;
    wxStaticText *m_text_action_;
    wxTextCtrl *m_text_task_;

    void setTask(std::string text);

    wxStaticText *m_text_predef_pos_;
    //wxStaticText *m_text_object;
    //wxStaticText *m_text_timeout;
    //wxStaticText *m_text_dist; // distance to closest pregrasp position

    //wxToggleButton *m_lock_cmap_;

    ros::ServiceServer service_start_;
    ros::ServiceServer service_timeout_;

    wxWindow *parent_;

    /*bool goal_away;
    bool goal_pregrasp;*/

    bool wait_for_start_;

    uint8_t traj_executed_;

    bool allow_repeat_;
    //bool cmap_locked_;

    std::string object_name_;
    std::string action_;

    //cob_client * cob_script;

    //bool gripper_initialized;

    void setButton(std::string but, bool state);

    void stateCallback(const srs_assisted_arm_navigation_msgs::AssistedArmNavigationState::ConstPtr& msg);

    std::vector<Preset> presets_;


private:

    DECLARE_EVENT_TABLE();

    enum {G_OPEN,G_CLOSE};

    //enum {S_INIT, S_NEW, S_PLAN, S_EXECUTE, S_PLAY, S_ADJUST, S_RESET, S_SUCCESS, S_FAILED, S_GOPEN, S_GCLOSE, S_LOOKAROUND, S_REFRESH};

    //void GripperThread(unsigned char action);
    void NewThread();
    void PlanThread();
    void ExecuteThread();
    //void LookThread();

    //boost::thread t_gripper;
    boost::thread t_new;
    boost::thread t_plan;
    boost::thread t_execute;
    //boost::thread t_look;

    boost::thread t_move_rel;
    boost::thread t_move_abs;

    bool checkService(std::string srv);

    //bool cob_script_inited;

    bool aco_;

    bool initialized_;

    bool state_received_;

    bool spacenav_pos_lock_;
    bool spacenav_or_lock_;

    ros::Subscriber arm_nav_state_sub_;

    wxCheckBox *m_pos_lock_;
    wxCheckBox *m_or_lock_;

    wxCheckBox *m_aco_;

    //wxToggleButton *b_switch_;

    boost::thread t_gui_update;
    void GuiUpdateThread();
    bool stop_gui_thread_;

    bool planning_started_;
    bool trajectory_planned_;

    bool arm_nav_called_;

};

} // namespace

#endif // BUT_ARMNAVIGATION_H