Пример #1
0
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;
}
Пример #2
0
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;
}
Пример #3
0
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);
}
Пример #4
0
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);
}
Пример #5
0
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();
	}
}