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