torso_gripper_sliders.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: 3D position interface for ground truth.
00023  * Author: Sachin Chitta and John Hsu
00024  * Date: 1 June 2008
00025  * SVN info: $Id$
00026  */
00027 
00028 #include <ros/ros.h>
00029 #include <ros/callback_queue.h>
00030 #include <ros/advertise_options.h>
00031 #include <std_msgs/Float64.h>
00032 #include <geometry_msgs/Twist.h>
00033 
00034 #include <actionlib/client/simple_action_client.h>
00035 #include <actionlib/client/terminal_state.h>
00036 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
00037 
00038 #include <boost/function.hpp>
00039 #include <boost/thread.hpp>
00040 
00041 /*#include <gazebo/Global.hh>
00042 #include <gazebo/XMLConfig.hh>
00043 #include <gazebo/HingeJoint.hh>
00044 #include <gazebo/SliderJoint.hh>
00045 #include <gazebo/Simulator.hh>
00046 #include <gazebo/gazebo.h>
00047 #include <gazebo/GazeboError.hh>
00048 #include <gazebo/ControllerFactory.hh>
00049 #include <gazebo/World.hh>
00050 */
00051 
00052 #include <QApplication>
00053 #include <QFont>
00054 #include <QLCDNumber>
00055 #include <QPushButton>
00056 #include <QSlider>
00057 #include <QVBoxLayout>
00058 #include <QWidget>
00059 #include "pr2_gazebo/qt_ros_slider.h"
00060 
00061 class MyWidget : public QWidget
00062 {
00063   public:
00064     MyWidget(QWidget *parent, ros::NodeHandle* rosnode)
00065         : QWidget(parent)
00066     {
00067         if (!ros::isInitialized()) {
00068           ROS_ERROR("ros node didn't start for some reason");
00069           return;
00070         }
00071         actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> *ac;
00072 
00073         QVBoxLayout *layout = new QVBoxLayout;
00074         QPushButton *quit = new QPushButton(tr("Quit"));
00075         quit->setFont(QFont("Times", 18, QFont::Bold));
00076         connect(quit, SIGNAL(clicked()), qApp, SLOT(quit()));
00077         layout->addWidget(quit);
00078 
00079         // /base_controller/command
00080         // /head_traj_controller/command
00081         // /l_arm_controller/command
00082         // /l_gripper_controller/command
00083         // /r_arm_controller/command
00084         // /r_gripper_controller/command
00085         // /torso_controller/command
00086 
00087         {
00088           std::string ac_name("/torso_controller/position_joint_action");
00089           ac = new actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction>(ac_name,true);
00090           while (!ac->waitForServer(ros::Duration(5.0))) ROS_INFO("waiting for torso action");
00091 
00092           QtRosSlider *lcd = new QtRosSlider("torso height",parent);
00093           lcd->setCallback(ac);
00094           lcd->min_value = 0.0;
00095           lcd->max_value = 0.31;
00096           lcd->setValue(0.0);
00097           layout->addWidget(lcd);
00098         }
00099         {
00100           std::string pub_name("/l_gripper_controller/command");
00101           QtRosSlider *lcd = new QtRosSlider("left gripper",parent);
00102           ros::Publisher pub = rosnode->advertise<pr2_controllers_msgs::Pr2GripperCommand>(pub_name,1);
00103           lcd->addGripperPublisher(pub);
00104           lcd->min_value = 0.0;
00105           lcd->max_value = 0.08;
00106           lcd->setValue(0.0);
00107           layout->addWidget(lcd);
00108         }
00109         {
00110           std::string pub_name("/r_gripper_controller/command");
00111           QtRosSlider *lcd = new QtRosSlider("right gripper",parent);
00112           ros::Publisher pub = rosnode->advertise<pr2_controllers_msgs::Pr2GripperCommand>(pub_name,1);
00113           lcd->addGripperPublisher(pub);
00114           lcd->min_value = 0.0;
00115           lcd->max_value = 0.08;
00116           lcd->setValue(0.0);
00117           layout->addWidget(lcd);
00118         }
00119 
00120         setLayout(layout);
00121     }
00122 };
00123 
00124 void QTThread(ros::NodeHandle* rosnode)
00125 {
00126   // start up qt
00127   int argc=0;
00128   char** argv = NULL;
00129   QApplication app(argc, argv);
00130   MyWidget widget(NULL,rosnode);
00131   widget.show();
00132   app.exec();
00133   rosnode->shutdown();
00134 }
00135 
00136 void QueueThread(ros::NodeHandle* rosnode)
00137 {
00138   static const double timeout = 0.01;
00139   ros::CallbackQueue srp_queue_;
00140   while (rosnode->ok())
00141   {
00142     srp_queue_.callAvailable(ros::WallDuration(timeout));
00143   }
00144   srp_queue_.clear();
00145   srp_queue_.disable();
00146 }
00147 
00149 int main(int argc, char** argv)
00150 {
00151   if (!ros::isInitialized())
00152   {
00153     int argc = 0;
00154     char** argv = NULL;
00155     ros::init(argc,argv,"controller_sliders");
00156   }
00157 
00158   ros::NodeHandle* rosnode_ = new ros::NodeHandle();
00159 
00160 
00161 
00162   boost::thread qt_thread_ = boost::thread( boost::bind( &QTThread,_1), rosnode_ );
00163   boost::thread callback_queue_thread_ = boost::thread( boost::bind( &QueueThread, _1 ), rosnode_ );
00164 
00165 
00166   callback_queue_thread_.join();
00167   qt_thread_.join();
00168 }
00169 
00170 


pr2_gazebo
Author(s): John Hsu
autogenerated on Thu Apr 24 2014 15:48:38