bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
makePlan(req.start, req.goal, resp.plan.poses);
resp.plan.header.stamp = ros::Time::now();
resp.plan.header.frame_id = frame_id_;
return true;
}
bool NavfnWithCostmap::makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp)
{
vector<PoseStamped> path;
req.start.header.frame_id = "/map";
req.goal.header.frame_id = "/map";
bool success = makePlan(req.start, req.goal, path);
resp.plan_found = success;
if (success) {
resp.path = path;
}
return true;
}
void NavfnWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
tf::Stamped<tf::Pose> global_pose;
cmap_->getRobotPose(global_pose);
vector<PoseStamped> path;
rm::PoseStamped start;
start.header.stamp = global_pose.stamp_;
start.header.frame_id = global_pose.frame_id_;
start.pose.position.x = global_pose.getOrigin().x();
start.pose.position.y = global_pose.getOrigin().y();
start.pose.position.z = global_pose.getOrigin().z();
start.pose.orientation.x = global_pose.getRotation().x();
start.pose.orientation.y = global_pose.getRotation().y();
start.pose.orientation.z = global_pose.getRotation().z();
start.pose.orientation.w = global_pose.getRotation().w();
makePlan(start, *goal, path);
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) {
return makePlan(start, goal, default_tolerance_, plan);
}
int main(int argc, char** argv) {
// Initialize ourselves as a ROS node.
ros::init(argc, argv, "planner");
ros::NodeHandle nh;
ros::Rate loop_rate(0.5); //hz
vis = nh.advertise<visualization_msgs::Marker> ("visualization_marker", 0);
// Subscribe to the map and people finder data.
ros::Subscriber map_sub;
ros::Subscriber people_finder_sub;
ros::Subscriber robot_pose_sub;
//ros::Subscriber costmap_sub;
//costmap_sub = nh.subscribe<nav_msgs::GridCells>("/move_base_node/local_costmap/obstacles", 1, updateCost);
map_sub = nh.subscribe<nav_msgs::OccupancyGrid> ("/map", 1, updateMap);
people_finder_sub = nh.subscribe<hide_n_seek::PeopleFinder> (
"fake_people_finder/people_finder", 1, updatePeople);
robot_pose_sub = nh.subscribe<nav_msgs::Odometry> (
"/base_pose_ground_truth", 1, updateRobotPose);
// map_inited = false;
pomdp.num_states = 400;
pomdp.num_lookahead = 1;
pomdp.states_inited = false;
//pomdp.current_state.pose.position.x = 0;
//pomdp.current_state.pose.position.y = 0;
ROS_INFO("States initialized.");
// spin the main loop
while (ros::ok()) {
ROS_INFO("service loop...");
{
State internalGoal,personGoal;
State s;
if (people.size() != 0) {
// If we have people to explore, exploring them is first priority.
personGoal.pose = people.at(0);
//this is a real world pose, now convert it into one of the pompdp state to calculate the euc dist from all poss states
personGoal.state_space_pose = realToStatePose(personGoal.pose);
s = makePlan(personGoal);
ROS_INFO("Person found at real(%f, %f)! Using them as the next goal.", personGoal.pose.position.x, internalGoal.pose.position.y);
} else if (pomdp.states_inited){
// get goal from POMDp with highest probability, if its an internal state no conversion needed
internalGoal = getMostLikelyState();
s = makePlan(internalGoal);
}
//internalGoal.state_space_pose = realToStatePose(internalGoal.pose);
// once we've initialized some states...
// make a plan
ROS_INFO("State received from makePlan: (%f, %f)", s.pose.position.x, s.pose.position.y);
if (people.size() != 0) {
people.erase(people.begin());
}
// print current goals
printGoals(vis);
// send a goal
visualization_msgs::Marker marker;
marker.header.frame_id = "map";//->header.frame_id;
marker.id = 67845;
marker.header.stamp = ros::Time();
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = s.pose.position.x;
marker.pose.position.y = s.pose.position.y;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = -1.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.z = 2.0;
marker.scale.x = 2.0;
marker.scale.y = 2.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
vis.publish(marker);
sendGoal(s);
ROS_INFO("Goal we're sending: (%f, %f)", s.pose.position.x, s.pose.position.y);
// set the current state
pomdp.current_state = s;
}
ros::spinOnce();
loop_rate.sleep();
}
}
