// 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); }