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