void NumberParser::run() {
prepare();
States state = States::Init;
Position before;
while ( true ) {
before = _input.position();
char c{};
if ( _input.eof() )
state = States::Quit;
else
c = _input.readChar();
switch ( state ) {
case States::Init:
state = stateInit( c );
break;
case States::Minus:
state = stateMinus( c );
break;
case States::Zero:
state = stateZero( c );
break;
case States::Digits:
state = stateDigits( c );
break;
case States::Point:
state = statePoint( c );
break;
case States::DecimalDigits:
state = stateDecimalDigits( c );
break;
case States::E:
state = stateE( c );
break;
case States::EPlusMinus:
state = stateEplusMinus( c );
break;
case States::EDigits:
state = stateEDigits( c );
break;
default:
break;
}
if ( state == States::Quit )
break;
}
_input.position( before );// return back the last character
if ( _isE )
postProcessing();
if ( _isMinus ) {
if ( isReal() )
_real *= -1;
if ( isInteger() )
_integer *= -1;
}
}
void publishTraj(){
visualization_msgs::Marker traj_visu;
traj_visu.header.frame_id="/map";
traj_visu.header.stamp=ros::Time::now();
traj_visu.ns="traj";
traj_visu.type=visualization_msgs::Marker::LINE_STRIP;
traj_visu.action=visualization_msgs::Marker::ADD;
traj_visu.id=1;
traj_visu.scale.x=0.05;
traj_visu.scale.y=0.05;
traj_visu.scale.z=0.05;
traj_visu.color.r = 1.0;
traj_visu.color.a = 1.0;
traj_visu.points.push_back(pose.point);
for(int i=0;i<current_path.size();++i){
geometry_msgs::Point p=statePoint(current_path[i]);
p.z=0.1;
traj_visu.points.push_back(p);
}
traj_pub_smooth.publish(traj_visu);
traj_visu.points.clear();
traj_visu.header.stamp=ros::Time::now();
traj_visu.ns="traj_raw";
traj_visu.id=2;
traj_visu.color.r = 0.0;
traj_visu.color.g = 1.0;
//traj_visu.points.push_back(pose.point);
for(int i=0;i<current_path_raw.size();++i){
geometry_msgs::Point p=statePoint(current_path_raw[i]);
p.z=0.1;
traj_visu.points.push_back(p);
}
traj_pub_raw.publish(traj_visu);
}
double obstacleProbability(State<2> state){
if((state-startState).norm()<inflation_radius)
return 0.0;
geometry_msgs::Point point=statePoint(state);
try{
occupancy_grid_utils::index_t index=occupancy_grid_utils::pointIndex(local_map->info,point);
int8_t val=local_map->data[index];
if(val == UNKNOWN){
//unknown space
return unknownSpaceProbability;
}else{
return 0.01*val;
}
}catch(occupancy_grid_utils::CellOutOfBoundsException e){
return 1.0;
}
}
bool isObstacle(State<2> state){
if((state-startState).norm()<inflation_radius)
return false;
geometry_msgs::Point point=statePoint(state);
try{
occupancy_grid_utils::index_t index=occupancy_grid_utils::pointIndex(local_map->info,point);
int8_t val=local_map->data[index];
//TODO (maybe use probabilities
//std::cout << val << std::endl;
if(val==UNKNOWN){
return unknownSpaceProbability>(1-epsilon);
}else{
if(val>100*(1-epsilon)){
return true;
}else{
return false;
}
}
}catch(occupancy_grid_utils::CellOutOfBoundsException e){
return true;
}
}
void Astar_planning(){
ROS_INFO("A star planning");
std::priority_queue<PQItem> open_list;
const unsigned num_cells = local_map->info.height*local_map->info.width;
std::vector<bool> seen(num_cells); // Default initialized to all false
const occupancy_grid_utils::index_t dest_ind = occupancy_grid_utils::pointIndex(local_map->info,goal.point);
const occupancy_grid_utils::index_t src_ind = occupancy_grid_utils::pointIndex(local_map->info,statePoint(startState));
open_list.push(PQItem(src_ind, 0, h(src_ind),src_ind));
std::vector<occupancy_grid_utils::index_t> parent(num_cells,0);
while (!open_list.empty()) {
const PQItem current = open_list.top();
open_list.pop();
const occupancy_grid_utils::Cell c = occupancy_grid_utils::indexCell(local_map->info, current.ind);
if (seen[current.ind])
continue;
parent[current.ind] = current.parent_ind;
seen[current.ind] = true;
ROS_DEBUG_STREAM_NAMED ("shortest_path_internal", " Considering " << c << " with cost " <<
current.g_cost << " + " << current.h_cost);
if (current.ind == dest_ind) {
std::cout << "solution found" << std::endl;
std::cout << "Visited " << std::count(seen.begin(), seen.end(), true) << " states out of " << num_cells << std::endl;;
std::deque<occupancy_grid_utils::index_t> path;
path.push_back(dest_ind);
occupancy_grid_utils::index_t last = dest_ind;
while (parent[last]!=src_ind){
path.push_front(parent[last]);
last=parent[last];
}
/*for(std::deque<occupancy_grid_utils::index_t>::iterator it=path.begin(),end=path.end();it!=end;++it){
std::cout << occupancy_grid_utils::indexCell(local_map->info, *it) << " -- ";
}
std::cout << std::endl;*/
current_path_raw.resize(path.size());
std::transform(path.begin(), path.end(), current_path_raw.begin(), &indexState);
/*std::cout << "Path length: " << current_path_raw.size() << std::endl;
std::cout << "Path cost: " << current.g_cost << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
planned=true;
return;
}
for (int d=-1; d<=1; d+=2) {
for (int vertical=0; vertical<2; vertical++) {
const int cx = c.x + d*(1-vertical);
const int cy = c.y + d*vertical;
if (cx>=0 && cy>=0) {
const occupancy_grid_utils::Cell c2((occupancy_grid_utils::coord_t) cx, (occupancy_grid_utils::coord_t) cy);
if (withinBounds(local_map->info, c2)) {
const occupancy_grid_utils::index_t ind = cellIndex(local_map->info, c2);
if (!isObstacle(pointState(indexPoint(ind))) && !seen[ind]) {
open_list.push(PQItem(ind, current.g_cost + g(ind), h(ind), current.ind));
}
//ROS_DEBUG_STREAM_COND_NAMED (g.data[ind]!=UNOCCUPIED, "shortest_path_internal",
// " Skipping cell " << indexCell(g.info, ind) <<
// " with cost " << (unsigned) g.data[ind]);
}
}
}
}
}
planning=false;
ROS_INFO("Planning failed");
}
