// Init Can-Configuration
bool srvCallback_Init(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_DEBUG("Service Callback init");
if(m_bisInitialized == false)
{
m_bisInitialized = initDrives();
//ROS_INFO("...initializing can-nodes...");
//m_bisInitialized = m_CanCtrlPltf->initPltf();
res.success.data = m_bisInitialized;
if(m_bisInitialized)
{
ROS_INFO("Can-Node initialized");
}
else
{
res.error_message.data = "initialization of can-nodes failed";
ROS_INFO("Initialization FAILED");
}
}
else
{
ROS_ERROR("...platform already initialized...");
res.success.data = false;
res.error_message.data = "platform already initialized";
}
return true;
}
bool NodeClass::can_init()
{
ROS_DEBUG("Service Callback init");
if(m_bisInitialized == false)
{
m_bisInitialized = initDrives();
last = ros::Time::now();
usleep(100000); //wait some time to ensure (now-last).toSec() isn't too small.
//ROS_INFO("...initializing can-nodes...");
if(m_bisInitialized)
{
bIsError = false;
ROS_INFO("base initialized");
}
else
{
ROS_ERROR("Initializing base failed");
}
}
else
{
ROS_WARN("...base already initialized...");
}
return true;
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
left_panel=new FilePanel(ui->centralWidget);
right_panel=new FilePanel(ui->centralWidget);
currentPanel=left_panel;
menu=new QMenu;
ui->gridLayout->addWidget(left_panel->layoutWidget,1,0,1,1);
ui->gridLayout->addWidget(right_panel->layoutWidget,1,1,1,1);
connect(left_panel,SIGNAL(tryOpen(QModelIndex)),this,SLOT(open(QModelIndex)));
connect(left_panel,SIGNAL(tryJump(QString)),this,SLOT(jumpTo(QString)));
connect(left_panel,SIGNAL(panelActivated(FilePanel*)),this,SLOT(setCurrentPanel(FilePanel*)));
connect(left_panel,SIGNAL(tryDelete()),this,SLOT(start_del()));
connect(right_panel,SIGNAL(tryOpen(QModelIndex)),this,SLOT(open(QModelIndex)));
connect(right_panel,SIGNAL(tryJump(QString)),this,SLOT(jumpTo(QString)));
connect(right_panel,SIGNAL(panelActivated(FilePanel*)),this,SLOT(setCurrentPanel(FilePanel*)));
connect(right_panel,SIGNAL(tryDelete()),this,SLOT(start_del()));
initDrives();
connect(right_panel,SIGNAL(tryMount(QString)),this,SLOT(mountDrive(QString)));
connect(left_panel,SIGNAL(tryMount(QString)),this,SLOT(mountDrive(QString)));
left_panel->setModel(new fileModel);
right_panel->setModel(new fileModel);
setup_contextMenu();
left_panel->setPopupMenu(menu);
right_panel->setPopupMenu(menu);
setup_toolbar();
setup_favoritiesPath();
left_panel->loadSettings("Left");
right_panel->loadSettings("Right");
connect(left_panel,SIGNAL(tryDrop(QList<QUrl>,QString,Qt::DropAction)),this,SLOT(drop(QList<QUrl>,QString,Qt::DropAction)));
connect(right_panel,SIGNAL(tryDrop(QList<QUrl>,QString,Qt::DropAction)),this,SLOT(drop(QList<QUrl>,QString,Qt::DropAction)));
connect(left_panel,SIGNAL(moveChecked(bool)),right_panel,SLOT(checkMove(bool)));
connect(right_panel,SIGNAL(moveChecked(bool)),left_panel,SLOT(checkMove(bool)));
QFileSystemWatcher *watcher=new QFileSystemWatcher;
watcher->addPath("/dev/disk/by-uuid");
connect(watcher,SIGNAL(directoryChanged(QString)),this,SLOT(drivesChanged(QString)));
}
// Constructor
NodeClass()
{
// initialization of variables
#ifdef __SIM__
m_bisInitialized = initDrives();
#else
m_bisInitialized = false;
#endif
/// Parameters are set within the launch file
// Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl.
if (n.hasParam("IniDirectory"))
{
n.getParam("IniDirectory", sIniDirectory);
ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
}
else
{
sIniDirectory = "Platform/IniFiles/";
ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
}
n.param<bool>("PublishEffort", m_bPubEffort, false);
if(m_bPubEffort) ROS_INFO("You have choosen to publish effort of motors, that charges capacity of CAN");
IniFile iniFile;
iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
// get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true);
iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true);
if(m_iNumMotors < 2 || m_iNumMotors > 8) {
m_iNumMotors = 8;
m_iNumDrives = 4;
}
#ifdef __SIM__
bl_caster_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_r_wheel_controller/command", 1);
br_caster_pub = n.advertise<std_msgs::Float64>("/base_br_caster_r_wheel_controller/command", 1);
fl_caster_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_r_wheel_controller/command", 1);
fr_caster_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_r_wheel_controller/command", 1);
bl_steer_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_rotation_controller/command", 1);
br_steer_pub = n.advertise<std_msgs::Float64>("/base_br_caster_rotation_controller/command", 1);
fl_steer_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_rotation_controller/command", 1);
fr_steer_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_rotation_controller/command", 1);
bl_caster_sub = n.subscribe("/base_bl_caster_r_wheel_controller/state", 1, &NodeClass::gazebo_bl_caster_Callback, this);
br_caster_sub = n.subscribe("/base_br_caster_r_wheel_controller/state", 1, &NodeClass::gazebo_br_caster_Callback, this);
fl_caster_sub = n.subscribe("/base_fl_caster_r_wheel_controller/state", 1, &NodeClass::gazebo_fl_caster_Callback, this);
fr_caster_sub = n.subscribe("/base_fr_caster_r_wheel_controller/state", 1, &NodeClass::gazebo_fr_caster_Callback, this);
bl_steer_sub = n.subscribe("/base_bl_caster_rotation_controller/state", 1, &NodeClass::gazebo_bl_steer_Callback, this);
br_steer_sub = n.subscribe("/base_br_caster_rotation_controller/state", 1, &NodeClass::gazebo_br_steer_Callback, this);
fl_steer_sub = n.subscribe("/base_fl_caster_rotation_controller/state", 1, &NodeClass::gazebo_fl_steer_Callback, this);
fr_steer_sub = n.subscribe("/base_fr_caster_rotation_controller/state", 1, &NodeClass::gazebo_fr_steer_Callback, this);
topicSub_GazeboJointStates = n.subscribe("/joint_states", 1, &NodeClass::gazebo_joint_states_Callback, this);
m_gazeboPos.resize(m_iNumMotors);
m_gazeboVel.resize(m_iNumMotors);
#else
m_CanCtrlPltf = new CanCtrlPltfCOb3(sIniDirectory);
#endif
// implementation of topics
// published topics
topicPub_JointState = n.advertise<sensor_msgs::JointState>("/joint_states", 1);
topicPub_ControllerState = n.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
topicPub_Diagnostic = n.advertise<diagnostic_msgs::DiagnosticStatus>("diagnostic", 1);
// subscribed topics
topicSub_JointStateCmd = n.subscribe("joint_command", 1, &NodeClass::topicCallback_JointStateCmd, this);
// implementation of service servers
srvServer_Init = n.advertiseService("init", &NodeClass::srvCallback_Init, this);
srvServer_ElmoRecorderConfig = n.advertiseService("ElmoRecorderConfig", &NodeClass::srvCallback_ElmoRecorderConfig, this);
srvServer_ElmoRecorderReadout = n.advertiseService("ElmoRecorderReadout", &NodeClass::srvCallback_ElmoRecorderReadout, this);
m_bReadoutElmo = false;
srvServer_Recover = n.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
srvServer_Shutdown = n.advertiseService("shutdown", &NodeClass::srvCallback_Shutdown, this);
}
