void DrivingWidget::SLO_ToggleDrivingMode() {
ui_->label_WaitingForACK->setText(tr("Waiting for ACK..."));
std_msgs::Bool enable_msg;
enable_msg.data = !controller_enabled_;
controller_enable_pub_.publish(enable_msg);
setGUIEnabled(false);
}
void ScopeSync::applyConfiguration()
{
DBG("ScopeSync::applyConfiguration");
#ifdef __DLL_EFFECT__
scopeSyncAsync.toggleUpdateProcessing(false);
#endif
parameterController->toggleAsyncUpdates(false);
setGUIEnabled(false);
parameterController->endAllParameterChangeGestures();
systemError = String::empty;
systemErrorDetails = String::empty;
if (ScopeSyncApplication::inPluginContext())
parameterController->storeParameterValues();
parameterController->reset();
// Firstly create the BCMParameter entries for each of the Host Parameters
ValueTree parameterTree = configuration->getParameters();
for (int i = 0; i < parameterTree.getNumChildren(); i++)
parameterController->addParameter(parameterTree.getChild(i));
parameterController->setupHostParameters();
#ifndef __DLL_EFFECT__
pluginProcessor->updateHostDisplay();
#endif // __DLL_EFFECT__
#ifndef __DLL_EFFECT__
parameterController->restoreParameterValues();
#else
scopeSyncAsync.snapshot();
scopeSyncAsync.toggleUpdateProcessing(true);
#endif // __DLL_EFFECT__
UserSettings::getInstance()->updateConfigurationLibraryEntry(getConfigurationFile().getFullPathName(),
getConfigurationFile().getFileName(),
getConfigurationRoot());
setGUIReload(true);
if (configurationManagerWindow != nullptr)
{
configurationManagerWindow->refreshContent();
configurationManagerWindow->restoreWindowPosition();
}
parameterController->toggleAsyncUpdates(true);
}
void DrivingWidget::handleControllerEnableACK(std_msgs::BoolConstPtr msg) {
if ( msg->data ) { // controller enabled
ui_->pushButton_ToggleDrivingMode->setText(tr("Disable Driving Mode"));
ui_->label_WaitingForACK->setText(tr("Driving Mode Active!"));
controller_enabled_ = true;
}
else { // controller disabled
ui_->pushButton_ToggleDrivingMode->setText(tr("Enable Driving Mode"));
ui_->label_WaitingForACK->setText(tr("Driving Mode Disabled!"));
ui_->pushButton_ShowCameraImage->setChecked(false);
controller_enabled_ = false;
}
setGUIEnabled(controller_enabled_);
}
DrivingWidget::DrivingWidget(QWidget *parent) :
QMainWindow(parent),
ui_(new Ui::DrivingWidget),
node_handle_private_("~")
{
ui_->setupUi(this);
// steering parameters
steering_speed_ = 0.0;
ignore_steering_limits_ = false;
// Driving control elements
current_state_.steering_wheel_angle = 0.0;
current_state_.head_pan = 0.0;
current_state_.head_tilt = 0.0;
current_state_.connection_loss = false;
current_state_.driving_counter = 0;
target_command_.all_hold = true;
target_command_.steering_wheel_angle = 0.0;
target_command_.drive_forward = false;
target_command_.head_pan = 0.0;
target_command_.head_tilt = 0.0;
controller_enabled_ = false;
head_tilt_speed_ = 0.0;
head_pan_speed_ = 0.0;
setGUIEnabled(false);
// Get camera image topic
node_handle_private_.param("camera_topic", camera_topic_, std::string("/head_cam/image_raw"));
// Get joypad values
joypad_command_sub_ = node_handle_.subscribe("/joy", 1, &DrivingWidget::handleJoyPadEvent, this);
// Get joypad ids
node_handle_private_.param("joypad_axis_steering", joypad_ids_[AXIS_STEERING], 0);
node_handle_private_.param("joypad_axis_head_pan", joypad_ids_[AXIS_HEAD_PAN], 2);
node_handle_private_.param("joypad_axis_head_tilt", joypad_ids_[AXIS_HEAD_TILT], 3);
node_handle_private_.param("joypad_axis_head_tilt_2", joypad_ids_[AXIS_HEAD_TILT_2], 4);
node_handle_private_.param("joypad_axis_head_pan_2", joypad_ids_[AXIS_HEAD_PAN_2], 5);
node_handle_private_.param("joypad_button_forward", joypad_ids_[BUTTON_FORWARD], 1);
node_handle_private_.param("joypad_button_all_hold", joypad_ids_[BUTTON_ALL_HOLD], 2);
node_handle_private_.param("joypad_button_steering_sensitivity_plus", joypad_ids_[BUTTON_STEERING_SENSITIVITY_PLUS], 7);
node_handle_private_.param("joypad_button_steering_sensitivity_minus", joypad_ids_[BUTTON_STEERING_SENSITIVITY_MINUS], 6);
node_handle_private_.param("joypad_button_head_sensitivity_plus", joypad_ids_[BUTTON_HEAD_SENSITIVITY_PLUS], 5);
node_handle_private_.param("joypad_button_head_sensitivity_minus", joypad_ids_[BUTTON_HEAD_SENSITIVITY_MINUS], 4);
node_handle_private_.param("joypad_button_head_to_default", joypad_ids_[BUTTON_HEAD_TO_DEFAULT], 11);
node_handle_private_.param("joypad_button_steering_to_default", joypad_ids_[BUTTON_STEERING_TO_DEFAULT], 10);
node_handle_private_.param("steering_sensitivity", steering_sensitivity_, -0.08);
node_handle_private_.param("head_tilt_sensitivity", head_tilt_sensitivity_, -0.05);
node_handle_private_.param("head_pan_sensitivity", head_pan_sensitivity_, 0.05);
// Setup driving commands
all_hold_enabled_sub_ = node_handle_.subscribe("driving_controller/all_hold", 1, &DrivingWidget::handleAllHoldEnabled, this);
driving_command_pub_ = node_handle_.advertise<humanoid_driving_controller::DrivingCommand>("driving_controller/driving_command", 1, false);
// Enable / Disable controller, Reset
controller_enable_pub_ = node_handle_.advertise<std_msgs::Bool>("driving_controller/controller_enable", 1, true);
controller_enable_ack_sub_ = node_handle_.subscribe("driving_controller/controller_enable_ack", 1, &DrivingWidget::handleControllerEnableACK, this);
// Get absolute steering angle from controller
driving_state_sub_ = node_handle_.subscribe("driving_controller/driving_state", 1, &DrivingWidget::handleNewDrivingState, this);
// Publish wheel angle
wheel_angle_pub_ = node_handle_.advertise<std_msgs::Float64>("driving_widget/wheel_angle_rad", 1, true);
// setup user interface
connect(ui_->pushButton_ConfirmSteeringSensitivity, SIGNAL(clicked()), this, SLOT(SLO_SteeringSensitivityConfirmed()));
connect(ui_->spinBox_SteeringSensitivity, SIGNAL(valueChanged(double)), this, SLOT(SLO_SteeringSensitivityChanged()));
connect(ui_->pushButton_ConfirmHeadTiltSensitivity, SIGNAL(clicked()), this, SLOT(SLO_HeadTiltSensitivityConfirmed()));
connect(ui_->spinBox_HeadTiltSensitivity, SIGNAL(valueChanged(double)), this, SLOT(SLO_HeadTiltSensitivityChanged()));
connect(ui_->pushButton_ConfirmHeadPanSensitivity, SIGNAL(clicked()), this, SLOT(SLO_HeadPanSensitivityConfirmed()));
connect(ui_->spinBox_HeadPanSensitivity, SIGNAL(valueChanged(double)), this, SLOT(SLO_HeadPanSensitivityChanged()));
connect(ui_->pushButton_ShowCameraImage, SIGNAL(toggled(bool)), this, SLOT(SLO_ShowCameraImage(bool)));
connect(ui_->pushButton_AllHold, SIGNAL(clicked(bool)), this, SLOT(SLO_AllHoldButtonChecked(bool)));
connect(ui_->pushButton_ToggleDrivingMode, SIGNAL(clicked()), this, SLOT(SLO_ToggleDrivingMode()));
connect(ui_->pushButton_OverrideLimits, SIGNAL(toggled(bool)), this, SLOT(SLO_OverrideLimits(bool)));
timer_.start(66, this);
// UI init
updateUI(true, true);
ui_->graphicsView_Wheels->setScene(&wheel_scene_);
}
