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