QGroupBox *win_object_prop::setup_port_orientation() { QGroupBox *groupBox = new QGroupBox(tr("Orientation")); QGridLayout *layout_alignment = new QGridLayout(); orientation_top = new QRadioButton(this); if (obj->text_position == 'T') orientation_top->setChecked(TRUE); QObject::connect(orientation_top, SIGNAL(toggled(bool)), this, SLOT(change_orientation(bool))); QHBoxLayout *layout_top = new QHBoxLayout(); layout_top->addStretch(0); layout_top->addWidget(orientation_top); layout_top->addStretch(0); orientation_left = new QRadioButton(this); if (obj->text_position == 'L') orientation_left->setChecked(TRUE); QObject::connect(orientation_left, SIGNAL(toggled(bool)), this, SLOT(change_orientation(bool))); orientation_right = new QRadioButton(this); if (obj->text_position == 'R') orientation_right->setChecked(TRUE); QObject::connect(orientation_right, SIGNAL(toggled(bool)), this, SLOT(change_orientation(bool))); orientation_bottom = new QRadioButton(this); if (obj->text_position == 'B') orientation_bottom->setChecked(TRUE); QObject::connect(orientation_bottom, SIGNAL(toggled(bool)), this, SLOT(change_orientation(bool))); QHBoxLayout *layout_bottom = new QHBoxLayout(); layout_bottom->addStretch(10); layout_bottom->addWidget(orientation_bottom); layout_bottom->addStretch(10); QSpacerItem *molla_sx = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum); QSpacerItem *molla_rx = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum); orientation_image = new QLabel(); layout_alignment->addItem(molla_sx,0,0, FALSE, TRUE); layout_alignment->addLayout(layout_top, 0, 2); layout_alignment->addWidget(orientation_left, 1, 1); layout_alignment->addWidget(orientation_right, 1, 3); layout_alignment->addLayout(layout_bottom, 2, 2); layout_alignment->addWidget(orientation_image, 1, 2); layout_alignment->addItem(molla_rx,0,4, FALSE, TRUE); groupBox->setLayout(layout_alignment); change_orientation(TRUE); //Mi serve per caricare l'immagine pin_text.png corretta return groupBox; }
//GOAL: visit every grid point, turn 360 and back int SimpleGridMotionLocalization::run() { ROS_INFO("RUNNNING LOCALIZATION"); bool planning_active = true; unsigned int i = 0; // for(unsigned int i=0;i<goal_points.size(); i++) while(ros::ok()) { if(planning_active) { //just to be safe, make sure the robot is stopped robot.wait_until_stopped(.5); ros::Duration(1.5).sleep(); //get current position/orientation from SLAM node update_current_position(); Point goal_point = goal_points[i]; int counter = 0; ///KEEP TRYING TO GET TO CURRENT GOAL POINT while(!is_at_point(goal_point) && counter < 1000) { ROS_ERROR("not at point: %f, %f", goal_point.x,goal_point.y); //############################## double desired_orientation = get_desired_orientation(goal_point); if(debug) { ROS_INFO("CurOrientation: %f, DesiredOrientation: %f", cur_orientation, desired_orientation); ros::Duration(3).sleep(); } double orientation_diff = desired_orientation - cur_orientation; //############################## //maybe move forwards/backwards trans_forward = true; //ASSUMES ROBOT IS AT 0 degrees orientation if(fabs(orientation_diff) > (.5 * M_PI)) { double new_diff = M_PI - fabs(orientation_diff) ; if(orientation_diff > 0) { new_diff = -new_diff; } orientation_diff = new_diff; trans_forward = false; if(debug) { ROS_INFO("DIFF < 90, move backwards: %f", orientation_diff); } }//if diff > 90 //############################## //decide if the angle is big enough to warrant turning to face goal if(!(fabs(orientation_diff) < ORIENTATION_THRESHOLD_RADIANS)) { //turn in increments so slam can keep up change_orientation(orientation_diff); }//if oreintation //find distance to goal point double dist = cur_point.distance_to_2d(goal_point); dist = dist*METERS_TO_MILLIMETERS; //move, in increments so the slam can keep up slam_move(dist, trans_forward); //give the slam a bit to catch up robot.wait_until_stopped(.5); ros::Duration(1).sleep(); //get the new position from slam update_current_position(); //reset orientation to 0 change_orientation(-cur_orientation); //give the slam a bit to catch up robot.wait_until_stopped(.5); ros::Duration(1).sleep(); //get the new position from slam update_current_position(); counter++; }//while not at goal point robot.wait_until_stopped(.5); //get the new position from slam update_current_position(); //now we are at the desired point,take the pictures slam_pause_client.call(empty_srv); if(max_rotate_180) { ROS_INFO("MAX ROTATE 180"); rotate(180, turn_res,true,true,2);//take and save data rotate(180,45,false,false);//turn back around rotate(180,45,false,false);//turn back around rotate(180, turn_res,true,true,2);//take and save data }//if rotate 180 at a time else { rotate(360, turn_res,true,true,.5);//take and save data rotate(360,45,false,false);//turn back around } robot.wait_until_stopped(.5); ros::Duration(4).sleep(); slam_resume_client.call(empty_srv); i++;//next goal point if(i >=goal_points.size()) { ROS_INFO("done planning"); planning_active = false; }//if i if(debug){ROS_INFO("done with current point");} }//if planning_active }//for i return 1; }//end run
void cb_key_down(Ewl_Widget *w, void *ev, void *data) { Ewl_Widget *curwidget; Ewl_Event_Key_Down *e; e = (Ewl_Event_Key_Down*)ev; int k = translate_key(e); curwidget=ewl_widget_name_find("pdfwidget"); switch(k) { case 0: ewl_pdf_page_next(EWL_PDF(curwidget)); resize_and_rescale(curscale); break; case 9: ewl_pdf_page_previous(EWL_PDF(curwidget)); resize_and_rescale(curscale); break; case 8: curscale+=((double)get_settings()->zoominc)/100.0; resize_and_rescale(curscale); break; case 7: curscale-=((double)get_settings()->zoominc)/100.0; resize_and_rescale(curscale); break; case 6: change_orientation(curwidget); resize_and_rescale(curscale); break; case 1: move_hscrollbar(EWL_SCROLLPANE(scrollpane), -1); break; case 2: move_hscrollbar(EWL_SCROLLPANE(scrollpane), 1); break; case 3: move_vscrollbar(EWL_SCROLLPANE(scrollpane), 1); break; case 4: move_vscrollbar(EWL_SCROLLPANE(scrollpane), -1); break; case 5: if(fitmode==0) fitmode=1; else if(fitmode==1) fitmode=0; calculate_margins(); resize_and_rescale(curscale); break; case K_RETURN: ewl_widget_show(menu); ewl_widget_focus_send(menu); break; case K_ESCAPE: ewl_main_quit(); break; default: return; } }