int main(int argc, char *argv[]) { //setup the ROS node ros::init(argc, argv, "ar_map_navigate_bumpers"); ros::NodeHandle n1, n2, n3, n4; vel_pub = n2.advertise<geometry_msgs::Twist>("/cmd_vel", 1); //SET QUEUE SIZE = 1!!! (keep only last msg) pospub = n4.advertise<position_tracker::Position>("/position", 1); //SET QUEUE SIZE = 1!!! (keep only last msg) pos_sub = n1.subscribe("/position", 1, positionCallback); bump_sub = n3.subscribe("/sensorPacket", 1, bumperCallback); loadMap(); loadWaypoints(); list<position_tracker::Position *>::iterator it; for(it = waypoints.begin(); it != waypoints.end(); ++it) //reach every single waypoint generated by the planner { goal_pos_x = (*it)->x; goal_pos_y = (*it)->y; //move to the next waypoint while(sqrt(pow(goal_pos_x - cur_pos.x, 2) + pow(goal_pos_y - cur_pos.y, 2)) > Tdist) { ros::spinOnce(); }//end while(current goal is not reached) }//end while(1) }
void Area::loadSAV(const Aurora::GFF3Struct &sav) { if (sav.hasField("CreatureList")) { const Aurora::GFF3Struct &creatures = sav.getStruct("CreatureList"); loadCreatures(creatures.getList("StaticList")); loadCreatures(creatures.getList("DynamicList")); } // TODO load crowd list if (sav.hasField("PlaceableList")) { const Aurora::GFF3Struct &placeables = sav.getStruct("PlaceableList"); loadPlaceables(placeables.getList("StaticList")); loadPlaceables(placeables.getList("DynamicList")); } // TODO load sound list if (sav.hasField("TriggerList")) { const Aurora::GFF3Struct &trigger = sav.getStruct("TriggerList"); loadTriggers(trigger.getList("StaticList")); loadTriggers(trigger.getList("DynamicList")); } if (sav.hasField("WaypointList")) { const Aurora::GFF3Struct &waypoints = sav.getStruct("WaypointList"); loadWaypoints(waypoints.getList("StaticList")); loadWaypoints(waypoints.getList("DynamicList")); } // TODO load projectile list // TODO load area of effect list // TODO load store list // TODO load apple list // TODO load camera list }
void Area::loadGIT(const Aurora::GFF3Struct &git) { // Waypoints if (git.hasField("WaypointList")) loadWaypoints(git.getList("WaypointList")); // Placeables if (git.hasField("Placeable List")) loadPlaceables(git.getList("Placeable List")); // Doors if (git.hasField("Door List")) loadDoors(git.getList("Door List")); }
void Area::loadGIT(const Aurora::GFF3Struct &git) { // Generic properties if (git.hasField("AreaProperties")) loadProperties(git.getStruct("AreaProperties")); // Waypoints if (git.hasField("WaypointList")) loadWaypoints(git.getList("WaypointList")); // Placeables if (git.hasField("Placeable List")) loadPlaceables(git.getList("Placeable List")); // Doors if (git.hasField("Door List")) loadDoors(git.getList("Door List")); // Creatures if (git.hasField("Creature List")) loadCreatures(git.getList("Creature List")); }
void WaypointLoaderNode::createLaneWaypoint(const std::string &file_path, autoware_msgs::lane *lane) { if (!verifyFileConsistency(file_path.c_str())) { ROS_ERROR("lane data is something wrong..."); return; } ROS_INFO("lane data is valid. publishing..."); FileFormat format = checkFileFormat(file_path.c_str()); std::vector<autoware_msgs::waypoint> wps; if (format == FileFormat::ver1) loadWaypointsForVer1(file_path.c_str(), &wps); else if (format == FileFormat::ver2) loadWaypointsForVer2(file_path.c_str(), &wps); else loadWaypoints(file_path.c_str(), &wps); lane->header.frame_id = "/map"; lane->header.stamp = ros::Time(0); lane->waypoints = wps; }
int main(int argc, char *argv[]) { ros::init(argc, argv, "simple_path_navigator_controller"); n1 = new ros::NodeHandle(); //JUST FOR TESTING... loadWaypoints(); cout << "Waypoints loaded" << endl; //call the service of path_navigator to set the waypoints ros::ServiceClient client = n1->serviceClient<path_navigator::setWaypoints>("set_waypoints"); path_navigator::setWaypoints srv; path_navigator::Waypoints *w = new path_navigator::Waypoints(); w->waypoints = waypoints; srv.request.w = *w; if(!client.call(srv)) { cout << "Failed to call service set_waypoints" << endl; } ros::spin(); }