SeExprEdNumberControl::SeExprEdNumberControl(int id,SeExprEdNumberEditable* editable)
:SeExprEdControl(id,editable,false),_numberEditable(editable)
{
// slider
float smin = editable->min, smax = editable->max;
if(!_numberEditable->isInt) { smin *= 1e5; smax *= 1e5; }
float srange = smax-smin;
_slider = new SeExprEdSlider(Qt::Horizontal,this);
_slider->setRange(int(smin),int(smax));
_slider->setTickInterval(std::max(1,int(srange/10)));
_slider->setSingleStep(std::max(1,int(srange/50)));
_slider->setPageStep(std::max(1,int(srange/10)));
_slider->setMinimumWidth(0);
_slider->setFixedHeight(16);
_slider->setFocusPolicy(Qt::ClickFocus);
hbox->addWidget(_slider, 3);
// edit box
_edit = new SeExprEdLineEdit(0,this);
_edit->setMinimumWidth(0);
_edit->setFixedHeight(16);
hbox->addWidget(_edit);
connect(_edit,SIGNAL(textChanged(int,const QString&)),SLOT(editChanged(int,const QString&)));
connect(_slider, SIGNAL(valueChanged(int)), SLOT(sliderChanged(int)));
// show current values
updateControl();
}
BioXASZebraInputView::BioXASZebraInputView(BioXASZebraInput *control, QWidget *parent) :
QWidget(parent)
{
// Initialize class variables.
control_ = 0;
// Create UI elements.
inputBox_ = new QSpinBox(this);
inputBox_->setRange(0, 63);
connect( inputBox_, SIGNAL(editingFinished()), this, SLOT(updateControl()) );
inputLabel_ = new QLabel(this);
inputLabel_->setAlignment(Qt::AlignCenter);
inputLabel_->setMinimumWidth(75);
inputStatusLabel_ = new QLabel(this);
// Create and set layouts.
QHBoxLayout *layout = new QHBoxLayout();
layout->setMargin(0);
layout->addWidget(inputBox_);
layout->addWidget(inputLabel_);
layout->addWidget(inputStatusLabel_);
setLayout(layout);
// Current settings.
setControl(control);
refresh();
}
SeExprEdStringControl::SeExprEdStringControl(int id,SeExprEdStringEditable* editable)
:SeExprEdControl(id,editable,false),_stringEditable(editable)
{
// make line edit
_edit=new QLineEdit();
_edit->setFixedHeight(20);
connect(_edit,SIGNAL(textChanged(const QString&)),SLOT(textChanged(const QString&)));
// make a button if we are a file or directory
if(_stringEditable->type=="file" || _stringEditable->type=="directory"){
QPushButton* button=new QPushButton();
button->setFixedSize(20,20);
hbox->addWidget(_edit,3);
hbox->addWidget(button,1);
if(_stringEditable->type=="directory"){
connect(button,SIGNAL(clicked()),SLOT(directoryBrowse()));
button->setIcon(QIcon(QPixmap(directoryXPM)));
} else if(_stringEditable->type=="file"){
connect(button,SIGNAL(clicked()),SLOT(fileBrowse()));
button->setIcon(QIcon(QPixmap(fileXPM)));
}
}else{
hbox->addWidget(_edit,3);
}
// update controls
updateControl();
}
//static
void LLFloaterSettingsDebug::onSettingSelect(LLUICtrl* ctrl)
{
LLComboBox* combo_box = (LLComboBox*)ctrl;
LLControlVariable* controlp = (LLControlVariable*)combo_box->getCurrentUserdata();
updateControl(controlp);
}
void NumberControl::setValue(float value) {
// std::cerr<<"In setValue "<<_id<<value<<std::endl;
if (fabs(_numberEditable->v - value) < 1e-5) return;
_numberEditable->v = value;
updateControl();
emit controlChanged(_id);
}
void LLFloaterSettingsDebug::onClickDefault()
{
if (mCurrentControlVariable)
{
mCurrentControlVariable->resetToDefault(true);
updateControl();
}
}
void LLFloaterSettingsDebug::draw()
{
// check for changes in control visibility, like RLVa does
if(mCurrentControlVariable && mCurrentControlVariable->isHiddenFromSettingsEditor() != mOldVisibility)
updateControl();
LLFloater::draw();
}
void LLFloaterSettingsDebug::draw()
{
LLComboBox* settings_combo = getChild<LLComboBox>("settings_combo");
LLControlVariable* controlp = (LLControlVariable*)settings_combo->getCurrentUserdata();
updateControl(controlp);
LLFloater::draw();
}
void VectorControl::setValue(int n, float value) {
if (n < 0 || n >= 3) return;
if (fabs(_numberEditable->v[n] - value) < 1e-5) return;
_numberEditable->v[n] = value;
if (_swatch) _swatch->setValue(_numberEditable->v);
updateControl();
emit controlChanged(_id);
}
void LLFloaterSettingsDebug::draw()
{
LLComboBox* settings_combo = getChild<LLComboBox>("settings_combo");
LLControlVariable* controlp = static_cast<LLControlVariable*>(settings_combo->getCurrentUserdata());
updateControl(controlp ? controlp->getCOAActive() : NULL);
LLFloater::draw();
}
void
V4LFrameSource::updateExposureTime(int husecs) {
#ifdef USE_V4L
try {
if (husecs > 0) {
updateControl(V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL);
updateControl(V4L2_CID_EXPOSURE_ABSOLUTE, husecs);
} else {
updateControl(V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_APERTURE_PRIORITY);
}
} catch (Exception &e) {
Log::warn("V4LFrameSource")
<< "Camera (" << m_deviceName
<< ") does not support exposure control"
<< std::endl;
}
#endif
}
// static
void LLFloaterSettingsDebug::onClickDefault()
{
LLComboBox* settings_combo = getChild<LLComboBox>("settings_combo");
LLControlVariable* controlp = (LLControlVariable*)settings_combo->getCurrentUserdata();
if (controlp)
{
controlp->resetToDefault(true);
updateControl(controlp);
}
}
static void updateControlWithAutoADC()
{
updateControl();
/*
#if (USE_AUDIO_INPUT==true)
adc_count = 0;
startSecondAudioADC();
#endif
*/
adcStartReadCycle();
}
void LLFloaterSettingsDebug::onSettingSelect()
{
mCurrentControlVariable = getControlVariable();
if (mOldControlVariable == mCurrentControlVariable) return;
// unbind change control signal from previously selected control
if(mOldControlVariable)
mOldControlVariable->getCommitSignal()->disconnect(boost::bind(&LLFloaterSettingsDebug::onSettingSelect, this));
// bind change control signal, so we can see updates to the current control in realtime
if(mCurrentControlVariable)
mCurrentControlVariable->getCommitSignal()->connect(boost::bind(&LLFloaterSettingsDebug::onSettingSelect, this));
mOldControlVariable = mCurrentControlVariable;
updateControl();
}
void GameState::update() {
updateControl(window);
updateCamera(camera);
double curTime = glfwGetTime();
double elapsedTime = curTime - prevTime;
prevTime = curTime;
for (int i = 0; i < actors.size(); i++) {
actors[i].step(elapsedTime);
}
if (curTime > timeToNextSphere) {
timeToNextSphere = curTime + 0.5;
spawnSphere();
}
checkCollisions();
camera->center += camera->direction * (float) elapsedTime
* getForwardVelocity();
glm::vec3 right = glm::cross(camera->direction, glm::vec3(0.0, 1.0, 0.0));
camera->center += right * (float) elapsedTime * getStrafeVelocity();
if(camera->center.y <= 0){
camera->center.y = 0;
}
if(camera->center.x >= XMAX){
camera->center.x = XMAX;
}
if(camera->center.x <= -XMAX){
camera->center.x = -XMAX;
}
if(camera->center.z >= ZMAX){
camera->center.z = ZMAX;
}
if(camera->center.z <= -ZMAX){
camera->center.z = -ZMAX;
}
printf("Current num of spheres is %d and Current num hit is %d\n", numCurSpheres, numSpheresHit);
}
SeExprEdVectorControl::SeExprEdVectorControl(int id,SeExprEdVectorEditable* editable)
:SeExprEdControl(id,editable,true),_numberEditable(editable)
{
if(_numberEditable->isColor){
_swatch=new SeExprEdCSwatchFrame(editable->v);
_swatch->setFixedWidth(38);
_swatch->setFixedHeight(20);
connect(_swatch,SIGNAL(swatchChanged(QColor)),this,SLOT(swatchChanged(QColor)));
hbox->addWidget(_swatch);
}
for(int i=0;i<3;i++){
QVBoxLayout* vbl=new QVBoxLayout();
hbox->addLayout(vbl);
vbl->setMargin(0);
vbl->setSpacing(0);
SeExprEdLineEdit* edit = new SeExprEdLineEdit(i, this);
vbl->addWidget(edit);
_edits[i]=edit;
edit->setMinimumWidth(0);
edit->setFixedHeight(16);
SeExprEdChannelSlider* slider = new SeExprEdChannelSlider(i, this);
vbl->addWidget(slider);
_sliders[i]=slider;
slider->setFixedHeight(6);
// set color
static const QColor rgb[3] = { QColor(128,64,64), QColor(64,128,64), QColor(64,64,128) };
if(_numberEditable->isColor) slider->setDisplayColor(rgb[i]);
connect(edit,SIGNAL(textChanged(int,const QString&)),SLOT(editChanged(int,const QString&)));
connect(slider, SIGNAL(valueChanged(int,float)), SLOT(sliderChanged(int,float)));
}
// update controls
updateControl();
}
void updateControls(void)
{
int i; for(i=0;i<INPUT_NUMBER;i++)updateControl(i);
}
void setSoundCaptions()
{
if(sndMode == TYPE_NORMAL)
{
switch(soundMode)
{
case SOUND_NEXTFILE:
updateControl(CONTROL_L, l_prevfile);
updateControl(CONTROL_R, l_nextfile);
break;
case SOUND_RANDFILE:
updateControl(CONTROL_L, l_back);
updateControl(CONTROL_R, l_randfile);
break;
default:
updateControl(CONTROL_L, NULL);
updateControl(CONTROL_R, NULL);
break;
}
setControlEnabled(CONTROL_L, true);
setControlEnabled(CONTROL_R, true);
}
else
{
updateControl(CONTROL_L, l_prevfile);
updateControl(CONTROL_R, l_nextfile);
if(curPlaylist.numEntries >= 2)
{
setControlEnabled(CONTROL_L, true);
setControlEnabled(CONTROL_R, true);
}
else
{
setControlEnabled(CONTROL_L, false);
setControlEnabled(CONTROL_R, false);
}
}
switch(getSourceFmt())
{
case SRC_STREAM_MP3:
case SRC_STREAM_OGG:
case SRC_STREAM_AAC:
updateControl(CONTROL_FORWARD, NULL);
break;
default:
if(getState() == STATE_PLAYING)
{
updateControl(CONTROL_FORWARD, l_pause);
}
else
{
updateControl(CONTROL_FORWARD, l_play);
}
break;
}
}
AlgorithmManager::AlgorithmManager(ros::NodeHandle& node):
algorithm_mode_(UNINITIALIZED)
{
ros::NodeHandle private_nh("~");
edo_core_msgs::JointsNumber::Request req;
edo_core_msgs::JointsNumber::Response res;
//subscribe to /movement_comm, published by state machine and /jnt_state from rosserial
ros::Subscriber move_control_sub = node.subscribe("machine_move", 100, &AlgorithmManager::moveCallback, this);
ros::Subscriber jog_control_sub = node.subscribe("machine_jog", 100, &AlgorithmManager::jogCallback, this);
ros::Subscriber jnt_state_subscriber = node.subscribe("machine_algo_jnt_state", 100, &AlgorithmManager::stateCallback, this);
ros::Subscriber jnt_calib_subscriber = node.subscribe("machine_jnt_calib", 100, &AlgorithmManager::calibCallback, this);
feedback_publisher_ = node.advertise<edo_core_msgs::MovementFeedback>("algo_movement_ack", 200);
cartesian_pose_pub_ = node.advertise<edo_core_msgs::CartesianPose>("cartesian_pose", 100);
algorithm_state_pub_ = node.advertise<std_msgs::Int8>("algorithm_state", 100);
//create a ROS Service Server
ros::ServiceServer get_jnts_number_srv = node.advertiseService("algo_jnt_number_srv", &AlgorithmManager::getJointsNumber, this);
ros::ServiceServer get_direct_kinematics_srv = node.advertiseService("algo_direct_kinematics_srv", &AlgorithmManager::getDirectKinematics, this);
ros::ServiceServer get_inverse_kinematics_srv = node.advertiseService("algo_inverse_kinematics_srv", &AlgorithmManager::getInverseKinematics, this);
robot_switch_control_server = node.advertiseService("algo_control_switch_srv", &AlgorithmManager::SwitchControl, this);
//publish /jnt_ctrl to rosserial, then joints
ros::Publisher robot_control_publisher = node.advertise<edo_core_msgs::JointControlArray>("algo_jnt_ctrl", 100);
//memset(&next_move_request_, 0, sizeof(edo_core_msgs::MovementFeedback));
memset(&target_joint_, 0, sizeof(ORL_joint_value));
memset(&target_cart_, 0, sizeof(ORL_cartesian_position));
memset(&hold_position_, 0, sizeof(ORL_joint_value));
memset(¤t_state_, 0, sizeof(ORL_joint_value));
delay_ = 255;
waiting_ = false;
jog_state_ = false;
jog_carlin_state_ = false;
move_cb_state_ = false;
pause_state_ = false;
interpolation_data_.resize(3);
first_time_ = true;
pkg_path_ = ros::package::getPath("edo_core_pkg");
jointCalib_=0;
private_nh.param<double>("controller_frequency", controller_frequency_, 100);
private_nh.param<double>("state_saturation_threshold", state_saturation_threshold_, 0.5);
private_nh.param<int>("interpolation_time_step", interpolation_time_step_, 1);
// Duration, callback, calback-owner, oneshot, autostart
timerCalib_ = private_nh.createTimer(ros::Duration(0.5), &AlgorithmManager::timerCallback, this, true, false);
if (!initializeORL())
{
ROS_ERROR("Impossible to inizialize ORL");
return;
}
noCartPose = false;
if(!getJointsNumber(req, res))
{
ROS_ERROR("Impossible to get joints number");
return;
}
joints_number_ = res.counter;
ROS_INFO("ORL controller initialized...");
ros::Rate loop_rate(controller_frequency_);
get_Strk(strk_);
//test con spin asincrono
ros::AsyncSpinner aspin(2);
aspin.start();
while (ros::ok())
{
if (algorithm_mode_ != SWITCHED_OFF){
//update the control command
updateControl();
//publish the last control commands
robot_control_publisher.publish(getCurrentControl());
}
loop_rate.sleep();
}
}
