QFESPIMB040SimpleCameraConfig::QFESPIMB040SimpleCameraConfig(QWidget* parent): QGroupBox(parent) { m_camViewID=0; m_extManager=NULL; camView=NULL; locked=false; restartPreview=false; m_magnification=1; m_lastUserPreview=""; // initialize raw image memory ... viewData.reset(); // create widgets and actions createWidgets(); createActions(); displayStates(QFESPIMB040SimpleCameraConfig::Disconnected); if (cmbAcquisitionDevice->count()<=0) displayStates(QFESPIMB040SimpleCameraConfig::Inactive); init(0, NULL, "", NULL); setCheckable(true); setChecked(true); }
void QFESPIMB040SimpleCameraConfig::startStopPreview() { //qDebug()<<"startStopPreview() actStartSTop="<<actStartStopPreview->isChecked(); if (viewData.camera) { int camIdx=viewData.usedCamera; if (viewData.camera->isCameraConnected(camIdx)) { if (actStartStopPreview->isChecked()) { displayStates(QFESPIMB040SimpleCameraConfig::Previewing); viewData.acqFrames=0; viewData.acqFramesQR=0; viewData.acqStarted.start(); viewData.acqStartedQR.start(); viewData.abort_continuous_acquisition=false; viewData.continuous_is_first=true; QString filename=""; filename=cmbPreviewConfiguration->currentConfigFilename(); QSettings* settings=new QSettings(filename, QSettings::IniFormat); viewData.camera->useCameraSettings(camIdx, *settings); camView->show(); startTimerSingleShot(true); delete settings; } else { displayStates(QFESPIMB040SimpleCameraConfig::Connected); } } } }
void QFESPIMB040SimpleCameraConfig::init(int camViewID, QFPluginServices* pluginServices, QString configDirectory, QFESPIMB040OpticsSetupBase* opticsSetup) { m_camViewID=camViewID; m_pluginServices=pluginServices; m_extManager=NULL; if (pluginServices) m_extManager=pluginServices->getExtensionManager(); cmbAcquisitionDevice->init(m_extManager); cmbPreviewConfiguration->init(configDirectory); if (m_pluginServices) { cmbPreviewConfiguration->init(m_pluginServices->getConfigFileDirectory()); } camView->init(camViewID, this, opticsSetup); setTitle(tr(" Camera %1: ").arg(camViewID+1)); displayStates(QFESPIMB040SimpleCameraConfig::Disconnected); if (cmbAcquisitionDevice->count()<=0) displayStates(QFESPIMB040SimpleCameraConfig::Inactive); }
bool QFESPIMB040SimpleCameraConfig::lockCamera(QFExtension** extension, QFExtensionCamera** ecamera, int* camera, QString* previewSettingsFilename) { if (locked || (!actDisConnect->isChecked()) || (!viewData.camera) || (!viewData.extension)) { *camera=-1; *extension=NULL; *ecamera=NULL; return false; } restartPreview=actStartStopPreview->isChecked(); actStartStopPreview->setChecked(false); stopPreview(); //QApplication::processEvents(); //QApplication::processEvents(); //QApplication::processEvents(); displayStates(QFESPIMB040SimpleCameraConfig::Locked); QString filename=""; *previewSettingsFilename=""; //if (cmbPreviewConfiguration->currentIndex()>=0) *previewSettingsFilename=cmbPreviewConfiguration->itemData(cmbPreviewConfiguration->currentIndex()).toString(); *previewSettingsFilename=cmbPreviewConfiguration->currentConfigFilename(); *extension=viewData.extension; *ecamera=viewData.camera; *camera=viewData.usedCamera; locked=true; return true; }
void QFESPIMB040SimpleCameraConfig::releaseCamera() { if (locked) { displayStates(QFESPIMB040SimpleCameraConfig::Connected); locked=false; if (restartPreview) { actStartStopPreview->setChecked(true); startStopPreview(); } } }
void QFESPIMB040SimpleCameraConfig::stopPreview() { //qDebug()<<"stopPreview()"; if (viewData.camera) { int camIdx=viewData.usedCamera; if (viewData.camera->isCameraConnected(camIdx)) { //qDebug()<<" cam connected"; viewData.abort_continuous_acquisition=true; actStartStopPreview->setChecked(false); displayStates(QFESPIMB040SimpleCameraConfig::Connected); viewData.abort_continuous_acquisition=true; stopTimer(); } } }
void QFESPIMB040SimpleCameraConfig::disConnectAcquisitionDevice() { if (cmbAcquisitionDevice->currentIndex()<0) { // if there are no devices registered: deactivate everything! displayStates(QFESPIMB040SimpleCameraConfig::Disconnected); return; } QFExtension* extension=cmbAcquisitionDevice->currentExtension(); QFExtensionCamera* cam=cmbAcquisitionDevice->currentExtensionCamera(); int camIdx=cmbAcquisitionDevice->currentCameraID(); //std::cout<<"disConnectAcquisitionDevice() dev="<<p.x()<<" cam="<<p.y()<<" cam*="<<cam<<" extension*="<<extension<<std::endl; if (cam && extension) { if (!cam->isCameraConnected(camIdx)) { // connect to a device connectDevice(cmbAcquisitionDevice->currentExtension(), cmbAcquisitionDevice->currentExtensionCamera(), cmbAcquisitionDevice->currentCameraID(), cmbAcquisitionDevice->currentCameraQObject()); if (!cam->isCameraConnected(camIdx)) { displayStates(QFESPIMB040SimpleCameraConfig::Disconnected); if (m_log) m_log->log_error(tr("error connecting to device %1, camera %2!\n").arg(extension->getName()).arg(camIdx)); } else { displayStates(QFESPIMB040SimpleCameraConfig::Connected); if (m_log) m_log->log_text(tr("connected to device %1, camera %2!\n").arg(extension->getName()).arg(camIdx)); cmbPreviewConfiguration->setDefaultAsCurrentConfig(); } } else { //disconnect from the current device and delete the settings widget displayStates(QFESPIMB040SimpleCameraConfig::Disconnected); //if (cam->isCameraConnected(camIdx)) { cam->disconnectCameraDevice(camIdx); //} if (m_log) m_log->log_text(tr("disconnected from device %1, camera %2!\n").arg(extension->getName()).arg(camIdx)); } } }
void QFESPIMB040SimpleCameraConfig::previewContinuous() { if (!camView) return; static int cnt=0; cnt++; if (viewData.camera) { int camIdx=viewData.usedCamera; QFExtension* extension=viewData.extension; QFExtensionCamera* cam=viewData.camera; //qDebug()<<"preview "<<cnt<<": abort="<<viewData.abort_continuous_acquisition; if (viewData.abort_continuous_acquisition) { displayStates(QFESPIMB040SimpleCameraConfig::Connected); //qDebug()<<"preview_abort "<<cnt; cnt--; return; } if (cam->isCameraConnected(camIdx)) { if (viewData.continuous_is_first) { viewData.continuous_is_first=false; } if (acquireSingle()) { camView->setPixelSize(cam->getCameraPixelWidth(camIdx)/m_magnification, cam->getCameraPixelHeight(camIdx)/m_magnification); camView->displayImage(viewData.rawImage, viewData.timestamp, viewData.exposureTime, viewData.parameters); } } else { if (m_log) m_log->log_error(tr("could not acquire frame, as device is not connected ...!\n")); } if (viewData.acqFramesQR%5==0) { double framerate=(double)viewData.acqFramesQR/(double)(viewData.acqStartedQR.elapsed()/1000.0); camView->displayCameraConfig(extension->getName()+" #"+QString::number(camIdx), framerate); if (viewData.acqStartedQR.elapsed()>10000) { viewData.acqStartedQR.start(); viewData.acqFramesQR=0; } } viewData.abort_continuous_acquisition=false; // start timer till next acquisition if (actStartStopPreview->isChecked()) { startTimerSingleShot(true); } cnt--; } }
int main(int argc, char **argv) { int motion = 0; std::string initialpath = ""; if (argc >= 2) { motion = atoi(argv[1]); if(argc >=3) { initialpath = argv[2]; } } printf("%d Motion %d\n", argc, motion); ros::init(argc, argv, "move_itomp"); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle("~"); robot_model_loader::RobotModelLoader robot_model_loader( "robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); planning_scene::PlanningScenePtr planning_scene( new planning_scene::PlanningScene(robot_model)); boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name; if (!node_handle.getParam("planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); try { planner_plugin_loader.reset( new pluginlib::ClassLoader<planning_interface::PlannerManager>( "moveit_core", "planning_interface::PlannerManager")); } catch (pluginlib::PluginlibException& ex) { ROS_FATAL_STREAM( "Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset( planner_plugin_loader->createUnmanagedInstance( planner_plugin_name)); if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) ROS_FATAL_STREAM("Could not initialize planner instance"); ROS_INFO_STREAM( "Using planning interface '" << planner_instance->getDescription() << "'"); } catch (pluginlib::PluginlibException& ex) { const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (std::size_t i = 0; i < classes.size(); ++i) ss << classes[i] << " "; ROS_ERROR_STREAM( "Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } loadStaticScene(node_handle, planning_scene, robot_model); /* Sleep a little to allow time to startup rviz, etc. */ ros::WallDuration sleep_time(1.0); sleep_time.sleep(); renderStaticScene(node_handle, planning_scene, robot_model); // We will now create a motion plan request // specifying the desired pose of the end-effector as input. planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; std::vector<robot_state::RobotState> robot_states; int state_index = 0; robot_states.push_back(planning_scene->getCurrentStateNonConst()); robot_states.push_back(robot_states.back()); Eigen::VectorXd start_trans, goal_trans; // set trajectory constraints std::vector<Eigen::VectorXd> waypoints; std::vector<std::string> hierarchy; // internal waypoints between start and goal if(initialpath.empty()) { hierarchy.push_back("base_prismatic_joint_x"); hierarchy.push_back("base_prismatic_joint_y"); hierarchy.push_back("base_prismatic_joint_z"); hierarchy.push_back("base_revolute_joint_z"); hierarchy.push_back("base_revolute_joint_y"); hierarchy.push_back("base_revolute_joint_x"); Eigen::VectorXd vec1; start_trans = Eigen::VectorXd(6); start_trans << 0.0, 1.0, 0.0,0,0,0; goal_trans = Eigen::VectorXd(6); goal_trans << 0.0, 2.5, 0.0,0,0,0; vec1 = Eigen::VectorXd(6); vec1 << 0.0, 1.5, 1.0,0,0,0; waypoints.push_back(vec1); vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.0, 2.0,0,0,0; waypoints.push_back(vec1); vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.5, 1.0,0,0,0; waypoints.push_back(vec1); } else { hierarchy = InitTrajectoryFromFile(waypoints, initialpath); start_trans = waypoints.front(); goal_trans = waypoints.back(); } setWalkingStates(robot_states[state_index], robot_states[state_index + 1], start_trans, goal_trans, hierarchy); for (int i = 0; i < waypoints.size(); ++i) { moveit_msgs::Constraints configuration_constraint = setRootJointConstraint(hierarchy, waypoints[i]); req.trajectory_constraints.constraints.push_back( configuration_constraint); } displayStates(robot_states[state_index], robot_states[state_index + 1], node_handle, robot_model); doPlan("whole_body", req, res, robot_states[state_index], robot_states[state_index + 1], planning_scene, planner_instance); visualizeResult(res, node_handle, 0, 1.0); ROS_INFO("Done"); planner_instance.reset(); return 0; }