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