void display_scene(int argc, char* argv[])
{
//typedefs
typedef mms::Mms_path_planner_example<> Planner;
//loading files from configuration file
Time_manager tm;
tm.write_time_log(std::string("start"));
Environment<> env(argc,argv);
tm.write_time_log(std::string("set environment"));
//loading scene from environment
Planner::Polygon_vec& workspace(env.get_workspace());
Planner::Extended_polygon my_robot(env.get_robot_a());
//load query
Planner::Reference_point q_s (env.get_source_configuration_a());
Planner::Reference_point q_t (env.get_target_configurations().front());
global_graphics->draw_polygons<Planner::K> (workspace, BLUE);
my_robot.move_absolute(q_s);
global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), GREEN);
my_robot.move_absolute(q_t);
global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), RED);
global_graphics->display();
return;
}
/*
* This method gets the markers information and it detects if the desire market is inside the list of markets detected.
* If the marker is detected, then it is calculate the position respect the frame of reference.
*/
bool NodeLandmarkGlobalLocalization::getRobotPosition(ar_track_alvar_msgs::AlvarMarker marker, tf::TransformListener *listener, float *counter,float *positions_x,float *positions_y,float *positions_yaw_x,float *positions_yaw_y){
if(marker.id == 3){
try{
geometry_msgs::PoseStamped temp_pose;
geometry_msgs::PoseStamped base_pose;
temp_pose.header = marker.header;
temp_pose.pose = marker.pose.pose;
listener->transformPose("scene",temp_pose,base_pose);
tf::Quaternion q_t(base_pose.pose.orientation.x,base_pose.pose.orientation.y,base_pose.pose.orientation.z,base_pose.pose.orientation.w);
tf::Matrix3x3 m(q_t);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
++*counter;
//X
*positions_x = base_pose.pose.position.x;
//Y
*positions_y = base_pose.pose.position.y;
//Decomposition of the angle in two components x and y
*positions_yaw_x = cosf(yaw);
*positions_yaw_y = sinf(yaw);
return true;
//std::cout<<"Info camera x \n"<<position_robot<<std::endl;
}catch(tf::TransformException& ex){
ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
}
}
return false;
}
void single_robot_planner_example(int argc, char* argv[])
{
//typedefs
typedef mms::Mms_path_planner_example<> Planner;
typedef Motion_sequence_gui_converter<Planner::K> Converter;
//loading files from configuration file
Time_manager tm;
tm.write_time_log(std::string("start"));
Environment<> env(argc,argv);
tm.write_time_log(std::string("set environment"));
//loading scene from environment
Planner::Polygon_vec& workspace(env.get_workspace());
Planner::Extended_polygon my_robot(env.get_robot_a());
//initialize the planner and preprocess
Planner planner(workspace, my_robot);
planner.preprocess();
//load query
Planner::Reference_point q_s (env.get_source_configuration_a());
Planner::Reference_point q_t (env.get_target_configurations().front());
//perform query
Motion_sequence<Planner::K> motion_sequence;
bool found_path = planner.query(q_s, q_t, motion_sequence);
if (!found_path)
std::cout<<"no path found :-("<<std::endl;
else
std::cout<<"path found :-)"<<std::endl;
//example of how to create a path that can be loaded by the GUI
if (found_path)
{
Converter::Path_3d path;
Converter converter;
converter(motion_sequence, std::back_inserter(path));
ofstream os("path.txt");
os <<path.size()<<std::endl;
for (unsigned int i(0); i < path.size(); ++i)
{
if (i == 0)
{
os << path[i].x <<" " << path[i].y <<" " << path[i].t <<" "
<< CGAL::to_double(q_t.get_location().x()) <<" " //we place the second robot at the target for ease of visualization
<< CGAL::to_double(q_t.get_location().y()) <<" " //we place the second robot at the target for ease of visualization
<< q_t.get_rotation().to_angle() //we place the second robot at the target for ease of visualization
<<std::endl;
}
else //i >0
{
os << path[i].x <<" " << path[i].y <<" " << path[i].t <<" " //first robot location
<< 0 <<" " << 0 <<" " << 0 <<" " //second robot is static
<< 2 //speed is arbitrarily chosen to be 2
<<std::endl;
}
}
}
return;
}
