//! Sends the command to start a given trajectory
 void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
 {
   
   // When to start the trajectory: 1s from now
   goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
   sendGoal(traj_client_, goal, nh);
 }
bool Cornering()
{
		goal_y += _row_width_;
		goal_x = x;
		goal_w += PI;
		sendGoal(goal_x, goal_y ,goal_w);
		while (!Begin_Row());
		return true;
}
bool Next_Goal()
{
	if (sin(w)>0){
		goal_x = _field_length_;
		}
	if (sin(w)<0){
		goal_x = 0;
		}
	sendGoal(goal_x, goal_y ,goal_w);
	return true;
}
int main(int argc, char** argv){
  ros::init(argc, argv, "slam_navigation");
  ros::NodeHandle nh;
  ros::NodeHandle nh1("~"); 				//subscribing to a private parameter server
  ros::Subscriber sub = nh.subscribe("odom", 1, &odomMsgs);				//subscribed to odometry msg
  ros::Subscriber local_costmap = nh.subscribe("/move_base/local_costmap/costmap",1,&costmap_grid);
 
  nh1.param("free_cells_", _free_cells_,80);
  nh1.param("field_length", _field_length_, 13);
  nh1.param("row_width", _row_width_, 0.75);
  nh1.param("number_of_rows", _num_rows_, 20);

  //tell the action client that we want to spin a thread by default
  //MoveBaseClient ac("move_base", true);
  
  ros::Rate loop_rate(10);

  //wait for the action server to come up
    
	
	sendGoal(_field_length_, 0 ,0);			//sending robot to its first goal
	ros::Time current_time, last_time;

  while (ros::ok()) {
		
		current_time = ros::Time::now();
		while(!End_Row());
	   
	  
	 	 dt = (current_time-last_time).toSec();
	  	last_time = current_time;
	  	
	  	loop_rate.sleep();
	  	ros::spinOnce();
     
  }
}
Beispiel #5
0
int main(int argc, char **argv){

        ros::init(argc, argv, "automap");
        ros::NodeHandle nh;

        // create dynamic reconfigure object
        dynamic_reconfigure::Server<automap::ExplorationConfig> server;
        dynamic_reconfigure::Server<automap::ExplorationConfig>::CallbackType f;
        automap::ExplorationConfig dynConfig;

        ros::Time initTime = ros::Time::now();

        nav_msgs::MapMetaData mapMetaData;
        cv::Mat map;
        simulation::telemetry_msg telemetry;
        sensor_msgs::ImagePtr edgeImageMsg;

        MoveBaseClient ac("move_base", true);
        while(!ac.waitForServer(ros::Duration(5.0)) && ros::ok())
        {
                ROS_INFO("Waiting for the move_base action server to come up...");
        }
        //control_On : motion control on/off / detection_On : sensing on/off /  NBV_On : nbv on/off
        automap::automap_ctrl_msg ctrlSignals;

        tf::TransformListener listener;
        geometry_msgs::Pose position;
        std::vector<double> rpy(3, 0.0);
        cv::Point gridPose;

        ros::Subscriber mapMetaSub = nh.subscribe<nav_msgs::MapMetaData>("map_metadata", 10, boost::bind(mapMetaCallback, _1, &mapMetaData));
        ros::Subscriber ctrlSub = nh.subscribe<automap::automap_ctrl_msg>("automap/ctrl_msg", 10, boost::bind(ctrlCallback, _1, &ctrlSignals));
        ros::Subscriber mapSub = nh.subscribe<nav_msgs::OccupancyGrid>("map", 10, boost::bind(mapCallback, _1, &map));
        ros::Publisher pathPub = nh.advertise<nav_msgs::Path>("pathtransformPlanner/path", 10);
        ros::Subscriber robotInfo = nh.subscribe<simulation::telemetry_msg>("telemetry", 10, boost::bind(telemetryCallback, _1, &telemetry));

        image_transport::ImageTransport it(nh);
        image_transport::Publisher edgePub = it.advertise("floatingEdges", 1);

        //wait for map - server
        ros::Duration d = ros::Duration(2, 0);
        ros::spinOnce();
        while(mapMetaData.resolution == 0 && ros::ok()) {
                ROS_INFO("Waiting for the map server to come up...");
                d.sleep();
                ros::spinOnce();
        }
        while(map.cols==0 && map.rows==0 && ros::ok()) {
                ROS_INFO("Waiting for the map server to come up...");
                d.sleep();
                ros::spinOnce();
        }

        PathtransformPlanner pPlanner(mapMetaData);
        ExplorationPlanner ePlanner(&pPlanner);

        // Begin: exploration metrics
        // array for drawing the robots path
        std::vector<cv::Point> explorationPath;
        // average planner time
        ros::Time planner_timer;
        double t_planner = 0;
        unsigned int t_planner_counter = 0;
        // average ex-planner time
        ros::Time explanner_timer;
        double t_explanner = 0;
        unsigned int t_explanner_counter = 0;
        // average busy loop time
        ros::Time loop_timer;
        double t_loop = 0;
        unsigned int t_loop_counter = 0;

        double timediff = 0;

        f = boost::bind(&configCallback, _1, _2, &pPlanner, &ePlanner, &dynConfig);
        server.setCallback(f);

        ctrlSignals.control_On=dynConfig.node_control_on;
        ctrlSignals.detection_On=dynConfig.node_sensing_on;
        ctrlSignals.NBV_On=dynConfig.node_use_nbv;

        bool finished = false;
        bool finalCheck = false;
        int retry = dynConfig.node_retries;
        cv::Mat old = cv::Mat::zeros(map.rows, map.cols, CV_8UC1);
        // Loop starts here:
        ros::Rate loop_rate(dynConfig.node_loop_rate);
        while(ros::ok() && !finished) {
                //start measure processing time
                loop_timer = ros::Time::now();

                //calculate information gain
                double gain = cv::norm(old, map, CV_L2);
                //Get Position Information...
                getPositionInfo("map", "base_footprint", listener, &position, &rpy);
                setGridPosition(position, mapMetaData, &gridPose);
                //Remember path
                explorationPath.push_back(gridPose);

                if(gain>dynConfig.node_information_gain || retry==0) {
                        ROS_INFO("Enough information gained: %lf",gain);
                        old = map.clone();
                        retry = dynConfig.node_retries;

                        //Feed pathtransformPlanner...
                        try{
                                ROS_INFO("Feeding PathtransformPlanner...");
                                //start measure processing time
                                planner_timer = ros::Time::now();
                                pPlanner.updateTransformMatrices(map, gridPose);
                                //stop measure processing time
                                timediff = (ros::Time::now()-planner_timer).toNSec()/NSEC_PER_SEC;
                                t_planner += timediff;
                                t_planner_counter++;
                                ROS_INFO("Path planning took: %lf",timediff);

                        }catch(std::exception& e) {
                                std::cout<<e.what()<<std::endl;
                        }
                        bool w = false;

                        if(ctrlSignals.detection_On) {
                                //start measure processing time
                                explanner_timer = ros::Time::now();
                                w = ePlanner.findBestPlan(map, gridPose, rpy[2], ctrlSignals.NBV_On);
                                //stop measure processing time
                                timediff = (ros::Time::now()-explanner_timer).toNSec()/NSEC_PER_SEC;
                                t_explanner += timediff;
                                t_explanner_counter++;
                                ROS_INFO("Exploration planning took: %lf",timediff);
                                if(!w && !finalCheck){
                                  finalCheck = true;
                                  continue;
                                }

                        }else{
                                w = true;
                        }



                        if(w) {
                                finalCheck =false;

                                ROS_INFO("Best next Plan found!");
                                std_msgs::Header genericHeader;
                                genericHeader.stamp = ros::Time::now();
                                genericHeader.frame_id = "map";


                                // send map with valid detected Edges
                                if(dynConfig.node_show_exploration_planner_result){
                                  cv::Mat out = ePlanner.drawFrontiers();
                                  edgeImageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out).toImageMsg();
                                  edgePub.publish(edgeImageMsg);
                                }


                                nav_msgs::Path frontierPath;
                                if(ctrlSignals.detection_On) {
                                        frontierPath = ePlanner.getBestPlan(genericHeader);
                                }


                                if(ctrlSignals.control_On && ctrlSignals.detection_On) {
                                        ROS_INFO("Sending Plan..");
                                        pathPub.publish(frontierPath);
                                        ros::spinOnce();
                                        ROS_INFO("Sending Goal..");
                                        sendGoal(frontierPath, ac);
                                }

                        }else{
                                ROS_INFO("Map exploration finished, aborting loop...");
                                //ac.waitForResult();
                                ac.cancelGoal();
                                ac.waitForResult();

                                std::string imgMetaPath = ros::package::getPath("automap") + "/data/output/map.yaml";
                                std::string imgStatPath = ros::package::getPath("automap") + "/data/output/exploration_statistics.txt";
                                std::string imgPath = ros::package::getPath("automap") + "/data/output/map.pgm";
                                std::string recordedPathPath = ros::package::getPath("automap") + "/data/output/path.png";

                                //Save explored map
                                std::vector<int> com_param;
                                com_param.push_back(CV_IMWRITE_PNG_COMPRESSION);
                                com_param.push_back(9);
                                try {
                                        cv::imwrite(imgPath, map, com_param);
                                        ROS_INFO("Map written to: %s", imgPath.c_str());

                                } catch (std::runtime_error& ex) {
                                        std::cout << "Exception converting img to PNG: " << ex.what() << std::endl;
                                }

                                //Save exploration path
                                cv::Mat pathMap = map.clone();
                                cv::cvtColor(pathMap, pathMap, CV_GRAY2RGB);
                                cv::polylines(pathMap, explorationPath, false, cv::Scalar(0, 0, 255), 1, 8);
                                try {
                                        cv::imwrite(recordedPathPath, pathMap, com_param);
                                        ROS_INFO("Path written to: %s", recordedPathPath.c_str());

                                } catch (std::runtime_error& ex) {
                                        std::cout << "Exception converting img to PNG: " << ex.what() << std::endl;
                                }

                                YAML::Emitter Y_out;
                                Y_out << YAML::BeginMap;
                                Y_out << YAML::Key << "image";
                                Y_out << YAML::Value << "map.pgm";
                                Y_out << YAML::Key << "resolution";
                                Y_out << YAML::Value << mapMetaData.resolution;
                                Y_out << YAML::Key << "origin";
                                Y_out << YAML::Flow;
                                Y_out << YAML::BeginSeq << mapMetaData.origin.position.x<<mapMetaData.origin.position.y
                                      <<mapMetaData.origin.position.z << YAML::EndSeq;
                                Y_out << YAML::Key << "negate";
                                Y_out << YAML::Value << 0;
                                Y_out << YAML::Key << "occupied_thresh";
                                Y_out << YAML::Value << 0.65;
                                Y_out << YAML::Key << "free_thresh";
                                Y_out << YAML::Value << 0.196;
                                Y_out << YAML::EndMap;


                                YAML::Emitter Y_out_statistics;
                                Y_out_statistics << YAML::BeginMap;
                                Y_out_statistics << YAML::Key << "driven distance during exploration(m)";
                                Y_out_statistics << YAML::Value << telemetry.radial_distance;
                                Y_out_statistics << YAML::Key << "time elapsed during exploration(s)";
                                Y_out_statistics << YAML::Value << (ros::Time::now()-initTime).toNSec()/NSEC_PER_SEC;
                                Y_out_statistics << YAML::Key << "average path planner processing time(s)";
                                Y_out_statistics << YAML::Value << t_planner/t_planner_counter;
                                Y_out_statistics << YAML::Key << "average exploration planner processing time(s)";
                                Y_out_statistics << YAML::Value << t_explanner/t_explanner_counter;
                                Y_out_statistics << YAML::Key << "average busy loop processing time(s)";
                                Y_out_statistics << YAML::Value << t_loop/t_loop_counter;

                                // calc quality of map
                                std::string godMapPath = ros::package::getPath("simulation") + "/data/map/map.pgm";
                                cv::Mat godMap = cv::imread(godMapPath,1);
                                cv::cvtColor(godMap, godMap, CV_RGB2GRAY);
                                double diffMap = cv::norm(godMap, map, CV_L2);

                                Y_out_statistics << YAML::Key << "in comparison to god map (L2Norm)";
                                Y_out_statistics << YAML::Value << diffMap;
                                Y_out_statistics << YAML::EndMap;

                                std::ofstream outfile_yaml(imgMetaPath);
                                try{
                                        outfile_yaml<<Y_out.c_str();
                                        ROS_INFO("MapMetaData written to: %s", imgMetaPath.c_str());
                                }catch (std::runtime_error& ex) {
                                        std::cout << "Exception writing .yaml-file: " << ex.what() << std::endl;
                                }
                                outfile_yaml.close();

                                std::ofstream outfile_statistics(imgStatPath);
                                try{
                                        outfile_statistics<<Y_out_statistics.c_str();
                                        ROS_INFO("Statistics data written to: %s", imgStatPath.c_str());
                                }catch (std::runtime_error& ex) {
                                        std::cout << "Exception writing .txt-file: " << ex.what() << std::endl;
                                }
                                outfile_statistics.close();

                                finished = true;
                                ros::shutdown();


                        }


                }else{
                        ROS_INFO("Not enough information gained: %lf",gain);
                        retry--;
                }
                // resend map with valid detected Edges
                if(dynConfig.node_show_exploration_planner_result){
                  cv::Mat out = ePlanner.drawFrontiers();
                  edgeImageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out).toImageMsg();
                  edgePub.publish(edgeImageMsg);
                }

                //stop measure processing time
                timediff = (ros::Time::now()-loop_timer).toNSec()/NSEC_PER_SEC;
                t_loop += timediff;
                t_loop_counter++;
                ROS_INFO("Whole loop took: %lf",timediff);

                ros::spinOnce();
                loop_rate.sleep();
        }

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