diff --git a/CMakeLists.txt b/CMakeLists.txt index 7750484..b123662 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,7 +73,7 @@ include_directories( ) ## Declare a C++ executable -add_executable(gsl_node src/gsl_algorithm.cpp src/gsl_server.cpp src/gsl_infotaxis.cpp) +add_executable(gsl_node src/gsl_algorithm.cpp src/gsl_server.cpp src/gsl_infotaxis.cpp src/visual_cpt.cpp) ## Add cmake target dependencies of the executable ## same as for the library above diff --git a/include/gsl_algorithm.h b/include/gsl_algorithm.h index 44e3430..e0fa4b9 100755 --- a/include/gsl_algorithm.h +++ b/include/gsl_algorithm.h @@ -39,12 +39,11 @@ class GSLAlgorithm { double distance_found; std::vector robot_poses_vector; double source_pose_x, source_pose_y; - double robot_pose_x, robot_pose_y; std::string results_file; nav_msgs::OccupancyGrid map_; //! Map geometry_msgs::PoseWithCovarianceStamped movingPose; //! Robot pose on the global frame referential - geometry_msgs::PoseWithCovarianceStamped current_robot_pose; //! Robot pose on the global frame referential + geometry_msgs::PoseWithCovarianceStamped current_pose; //! Robot pose on the global frame referential MoveBaseClient mb_ac; //! Move Base Action Server. //Subscriptions diff --git a/include/gsl_infotaxis.h b/include/gsl_infotaxis.h index a9b1061..0ef6a1b 100755 --- a/include/gsl_infotaxis.h +++ b/include/gsl_infotaxis.h @@ -3,11 +3,11 @@ #include #include #include -#include #include #include #include #include +#include typedef actionlib::SimpleActionClient MoveBaseClient; enum class Infotaxis_state {WAITING_FOR_MAP, STOP_AND_MEASURE, MOVING}; @@ -17,18 +17,15 @@ class Cell { Cell(bool free, double x, double y, double weight); ~Cell(){}; bool free; - double x, y, weight, auxWeight; - double distance; + double x, y, weight, auxWeight, distance; }; struct WindVector { - int i; - int j; - double speed; - double angle; + int i, j; + double speed, angle; }; -class InfotaxisGSL:public GSLAlgorithm { +class InfotaxisGSL:public GSLAlgorithm, public VisualCPT { public: InfotaxisGSL(ros::NodeHandle *nh); ~InfotaxisGSL(); @@ -67,10 +64,9 @@ class InfotaxisGSL:public GSLAlgorithm { //Estimations double stdev_hit; double stdev_miss; - void estimateProbabilities(std::vector >& map, - bool hit, - double wind_direction, + void estimateProbabilities(std::vector >& map, bool hit, double wind_direction, Eigen::Vector2i robot_pos); + void propagateProbabilities(std::vector >& map, std::unordered_set, boost::hash< std::pair > >& openSet, std::unordered_set, boost::hash< std::pair > >& closedSet, @@ -110,16 +106,12 @@ class InfotaxisGSL:public GSLAlgorithm { std::vector > cells; ros::Publisher probability_markers; ros::Publisher entropy_reporter; - ros::Publisher switch_marker; - ros::Publisher hit_marker; - void switch_notify(); - void hit_notify(); + Eigen::Vector2i currentPosIndex; //Auxiliary functions - visualization_msgs::Marker emptyMarker(); + // visualization_msgs::Marker emptyMarker(); void showWeights(); - Eigen::Vector3d valueToColor(double val, double low, double high); Eigen::Vector2i coordinatesToIndex(double x, double y); Eigen::Vector2d indexToCoordinates(double i, double j); double gaussian(double distance, double sigma); diff --git a/include/visual_cpt.h b/include/visual_cpt.h new file mode 100644 index 0000000..6e3f78d --- /dev/null +++ b/include/visual_cpt.h @@ -0,0 +1,24 @@ +#ifndef VISUAL_CPT +#define VISUAL_CPT +#include +#include +#include + +class VisualCPT { + public: + VisualCPT(ros::NodeHandle *nh); + ~VisualCPT(); + + void switch_notify(int planning_mode); + void hit_notify(bool gasHit); + void plotplot(float haha); + Eigen::Vector3d valueToColor(double val, double low, double high); + visualization_msgs::Marker emptyMarker(int numCells); + + protected: + ros::NodeHandle *nh_; + ros::Publisher switch_marker; + ros::Publisher hit_marker; + ros::Publisher test_marker; +}; +#endif \ No newline at end of file diff --git a/package.xml b/package.xml index be43cb1..9c05df7 100755 --- a/package.xml +++ b/package.xml @@ -6,7 +6,7 @@ - Pepe Ojeda + Nhat Luong GPLv3 diff --git a/src/gsl_algorithm.cpp b/src/gsl_algorithm.cpp index 72ff75a..8820ec5 100755 --- a/src/gsl_algorithm.cpp +++ b/src/gsl_algorithm.cpp @@ -29,14 +29,12 @@ GSLAlgorithm::GSLAlgorithm(ros::NodeHandle *nh) : nh_(nh), mb_ac("move_base", tr nh->param("distance_found", distance_found, 0.5); nh->param("ground_truth_x", source_pose_x, 1.5); nh->param("ground_truth_y", source_pose_y, 3.0); - nh->param("robot_pose_x", robot_pose_x, 0.0); - nh->param("robot_pose_y", robot_pose_y, 0.0); //====================== Subscribers ====================== localization_sub_ = nh_->subscribe(robot_location_topic,100,&GSLAlgorithm::localizationCallback,this); //======================= Services ======================== - mb_client = nh_->serviceClient("/move_base/GlobalPlanner/make_plan"); + mb_client = nh_->serviceClient("/move_base/NavfnROS/make_plan"); inMotion = false; inExecution = false; } @@ -49,8 +47,8 @@ GSLAlgorithm::~GSLAlgorithm(){} //================================================================================= void GSLAlgorithm::localizationCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) { - current_robot_pose = *msg; //keep the most recent robot pose - robot_poses_vector.push_back(current_robot_pose); //Keep all poses + current_pose = *msg; //keep the most recent robot pose + robot_poses_vector.push_back(current_pose); //Keep all poses } @@ -86,39 +84,39 @@ float GSLAlgorithm::get_average_vector(std::vector const &v) { } +/* Check if next location to move inside the map or not */ bool GSLAlgorithm::checkGoal(move_base_msgs::MoveBaseGoal * goal) { - ROS_INFO("[DEBUG] Checking Goal [%.2f, %.2f] in map frame", goal->target_pose.pose.position.x, goal->target_pose.pose.position.y); - - //1. Get dimensions of OccupancyMap + geometry_msgs::PoseStamped target = goal->target_pose; + float target_x = target.pose.position.x; + float target_y = target.pose.position.y; + ROS_INFO("Checking Goal [%.2f, %.2f] in map frame", target_x, target_y); + // Check that goal falls inside the map float map_min_x = map_.info.origin.position.x; float map_max_x = map_.info.origin.position.x + map_.info.width*map_.info.resolution; float map_min_y = map_.info.origin.position.y; float map_max_y = map_.info.origin.position.y + map_.info.height*map_.info.resolution; - - //2. Check that goal falls inside the map - if (goal->target_pose.pose.position.x < map_min_x || goal->target_pose.pose.position.x > map_max_x || - goal->target_pose.pose.position.y < map_min_y || goal->target_pose.pose.position.y > map_max_y) { - ROS_INFO("[DEBUG] Goal is out of map dimensions"); + if (target_x < map_min_x || target_x > map_max_x || target_y < map_min_y || target_y > map_max_y) { + ROS_ERROR("Goal is out of map dimensions!!!"); return false; } - //3. Use Move Base Service to declare a valid navigation goal + //3. Use MoveBase service to declare a valid goal and get path from robot to it nav_msgs::GetPlan mb_srv; geometry_msgs::PoseStamped start_point; start_point.header.frame_id = "map"; start_point.header.stamp = ros::Time::now(); - start_point.pose = current_robot_pose.pose.pose; + start_point.pose = current_pose.pose.pose; + mb_srv.request.start = start_point; - mb_srv.request.tolerance=0.0; - mb_srv.request.start.header.frame_id = "map"; - mb_srv.request.goal = goal->target_pose; + mb_srv.request.goal = target; + mb_srv.request.tolerance = 0.1; //get path from robot to candidate. if( mb_client.call(mb_srv) && mb_srv.response.plan.poses.size()>1) { return true; } else { - ROS_ERROR("Unable to reach [%.2f, %.2f] with MoveBase", goal->target_pose.pose.position.x, goal->target_pose.pose.position.y); + ROS_ERROR("Unable to reach [%.2f, %.2f] with MoveBase", target_x, target_y); return false; } } @@ -134,8 +132,8 @@ int GSLAlgorithm::checkSourceFound() { } //Check the distance from robot to source - double Ax = current_robot_pose.pose.pose.position.x - source_pose_x; - double Ay = current_robot_pose.pose.pose.position.y - source_pose_y; + double Ax = current_pose.pose.pose.position.x - source_pose_x; + double Ay = current_pose.pose.pose.position.y - source_pose_y; double dist = sqrt(pow(Ax,2) + pow(Ay,2)); if (dist < distance_found) { ROS_INFO("SUCCESS -> Time spent (%.3f s)", time_spent.toSec()); diff --git a/src/gsl_infotaxis.cpp b/src/gsl_infotaxis.cpp index d2b16a5..663f428 100755 --- a/src/gsl_infotaxis.cpp +++ b/src/gsl_infotaxis.cpp @@ -3,18 +3,18 @@ #include "std_msgs/Float32.h" Cell::Cell(bool f, double a, double b, double c) { - free=f; - x=a; - y=b; - weight=c; - auxWeight=0; - distance=0; + x = a; + y = b; + free = f; + weight = c; + auxWeight = 0; + distance = 0; } -InfotaxisGSL::InfotaxisGSL(ros::NodeHandle *nh) : GSLAlgorithm(nh) { +InfotaxisGSL::InfotaxisGSL(ros::NodeHandle *nh) : GSLAlgorithm(nh), VisualCPT(nh){ nh->param("th_gas_present", th_gas_present, 0.3); nh->param("th_wind_present", th_wind_present, 0.03); - nh->param("stop_and_measure_time", stop_and_measure_time, 3); + nh->param("stop_and_measure_time", stop_and_measure_time, 2); nh->param("scale", scale, 65); //scale for dynamic map reduction nh->param("convergence_thr", convergence_thr, 0.5); //threshold for source declaration nh->param("stdev_hit", stdev_hit, 1.0); //standard deviation of hit and miss? @@ -28,10 +28,8 @@ InfotaxisGSL::InfotaxisGSL(ros::NodeHandle *nh) : GSLAlgorithm(nh) { map_sub_ = nh->subscribe(map_topic, 1, &InfotaxisGSL::mapCallback, this); probability_markers = nh->advertise("probability_markers", 10); - switch_marker = nh->advertise("switch_marker", 10); - hit_marker = nh->advertise("hit_marker", 10); entropy_reporter = nh->advertise("entropy_reporter", 10); - + // Init State gasHit = false; number_revisited = 0; @@ -53,34 +51,42 @@ InfotaxisGSL::~InfotaxisGSL() {} // see http://docs.ros.org/api/nav_msgs/html/msg/MapMetaData.html void InfotaxisGSL::mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) { map_ = *msg; + float map_originX = map_.info.origin.position.x; + float map_originY = map_.info.origin.position.y; + float map_height = map_.info.height; + float map_width = map_.info.width; + float map_resolution = map_.info.resolution; ROS_INFO("Got the map of the environment!"); ROS_INFO("--------------INFOTAXIS GSL---------------"); ROS_INFO("Occupancy Map dimensions:"); //i is y(height), j is x(width) ROS_INFO("x_min:%.2f x_max:%.2f / y_min:%.2f y_max:%.2f", - map_.info.origin.position.x, map_.info.origin.position.x + map_.info.width*map_.info.resolution, - map_.info.origin.position.y, map_.info.origin.position.y + map_.info.height*map_.info.resolution); + map_originX, map_originX + map_width*map_resolution, + map_originY, map_originY + map_height*map_resolution); ROS_INFO("------------------------------------------"); - std::vector> map_origin(map_.info.height,std::vector(map_.info.width)); + std::vector> map_origin(map_height,std::vector(map_width)); //invert from 0 is free to 1 is free int index=0; for(int i=0; iraw); } @@ -125,7 +130,7 @@ void InfotaxisGSL::gasCallback(const olfaction_msgs::gas_sensorPtr& msg) { void InfotaxisGSL::windCallback(const olfaction_msgs::anemometerPtr& msg) { //1. Add obs to the vector of the last N wind speeds - float downWind_direction = angles::normalize_angle(msg->wind_direction); + float downWind_direction = angles::normalize_angle(msg->wind_direction + M_PI); //simulation wind geometry_msgs::PoseStamped anemometer_downWind_pose, map_downWind_pose; //Transform from anemometer ref_system to map ref_system using TF try { anemometer_downWind_pose.header.frame_id = msg->header.frame_id; @@ -133,7 +138,7 @@ void InfotaxisGSL::windCallback(const olfaction_msgs::anemometerPtr& msg) { anemometer_downWind_pose.pose.position.y = 0.0; anemometer_downWind_pose.pose.position.z = 0.0; anemometer_downWind_pose.pose.orientation = tf::createQuaternionMsgFromYaw(downWind_direction); - tf_.transformPose("map", anemometer_downWind_pose, map_downWind_pose); //doan nay meo hieu sao phai cos 2 objrect + tf_.transformPose("map", anemometer_downWind_pose, map_downWind_pose); } catch(tf::TransformException &ex) { ROS_ERROR("InfotaxisPT - Error: %s", ex.what()); @@ -169,35 +174,40 @@ void InfotaxisGSL::getGasWindObservations() { gas_vector.clear(); wind_spd_vector.clear(); - previous_robot_pose=Eigen::Vector2d(current_robot_pose.pose.pose.position.x, current_robot_pose.pose.pose.position.y); + previous_robot_pose=Eigen::Vector2d(current_pose.pose.pose.position.x, current_pose.pose.pose.position.y); previous_state = current_state; - + ROS_ERROR("avg_gas=%.2f | avg_wind_spd=%.2f | avg_wind_dir=%.2f", avg_concentration, avg_wind_spd, avg_wind_dir); + if (avg_concentration > th_gas_present && avg_wind_spd > th_wind_present) { //Gas & wind + ROS_WARN("GAS HIT!!! New state --> MOVING"); gasHit = true; - // previous_robot_pose = Eigen::Vector2d(current_robot_pose.pose.pose.position.x, current_robot_pose.pose.pose.position.y); //register where you were before moving + hit_notify(gasHit); + // previous_robot_pose = Eigen::Vector2d(current_pose.pose.pose.position.x, current_pose.pose.pose.position.y); //register where you were before moving estimateProbabilities(cells, gasHit, avg_wind_dir, currentPosIndex); current_state = Infotaxis_state::MOVING; - ROS_WARN("GAS HIT!!! New state --> MOVING"); } else if (avg_concentration > th_gas_present) { //Only gas + ROS_WARN("GAS, BUT NO WIND New state --> MOVING"); gasHit=true; - // previous_robot_pose=Eigen::Vector2d(current_robot_pose.pose.pose.position.x, current_robot_pose.pose.pose.position.y); + hit_notify(gasHit); + // previous_robot_pose=Eigen::Vector2d(current_pose.pose.pose.position.x, current_pose.pose.pose.position.y); previous_state=current_state; current_state = Infotaxis_state::MOVING; - ROS_WARN("GAS, BUT NO WIND New state --> MOVING"); + } else { //Nothing gasHit=false; + ROS_WARN("NOTHING!!!! New state --> MOVING"); + hit_notify(gasHit); estimateProbabilities(cells, gasHit, avg_wind_dir, currentPosIndex); previous_state=current_state; current_state = Infotaxis_state::MOVING; - ROS_WARN("NOTHING!!!! New state --> MOVING"); + } - hit_notify(); } } @@ -206,18 +216,19 @@ void InfotaxisGSL::estimateProbabilities(std::vector >& map, b std::unordered_set< std::pair, boost::hash > > activePropagationSet; std::unordered_set< std::pair, boost::hash > > closedPropagationSet; - int i=robot_pos.x(), j=robot_pos.y(); - - int oI=std::max(0,i-1); - int fI=std::min((int) map.size()-1,i+1); - int oJ=std::max(0,j-1); - int fJ=std::min((int) map[0].size()-1,j+1); + int i = robot_pos.x(); + int j = robot_pos.y(); + int oI = std::max(0,i-1); + int fI = std::min((int)map.size()-1, i+1); + int oJ = std::max(0,j-1); + int fJ = std::min((int)map[0].size()-1, j+1); - double sum=0; - //estimate the probabilities for the immediate 8 neighbours + //estimate the probabilities for the 8 neighbours Eigen::Vector2d coordR = indexToCoordinates(i,j); - double upwind_dir = angles::normalize_angle(wind_direction+M_PI); - double move_dir = atan2((previous_robot_pose.y()-coordR.y()),(previous_robot_pose.x()-coordR.x()))+M_PI; + double upwind_dir = angles::normalize_angle(wind_direction + M_PI); + double move_dir = atan2((coordR.y() - previous_robot_pose.y()),(coordR.x()-previous_robot_pose.x())); + move_dir += M_PI; //assign higher probability to opposite moving direction when miss (just like upwind dir) + double maxHit = gaussian(0,stdev_hit); double maxMiss = gaussian(0,stdev_miss); @@ -227,30 +238,31 @@ void InfotaxisGSL::estimateProbabilities(std::vector >& map, b if(c!=j || r!=i) { Eigen::Vector2d coordP = indexToCoordinates(r,c); double dist; - double cell_vector = atan2((coordR.y()-coordP.y()),(coordR.x()-coordP.x())); + double cell_vector = angles::normalize_angle(atan2((coordP.y()-coordR.y()),(coordP.x()-coordR.x()))); if(hit) { dist=gaussian(atan2(sin(upwind_dir-cell_vector), cos(upwind_dir-cell_vector)),stdev_hit); - }else { + } + else { dist=gaussian(atan2(sin(move_dir-cell_vector), cos(move_dir-cell_vector)), stdev_miss); } - map[r][c].weight=dist*map[r][c].weight; + activePropagationSet.insert(std::pair(r,c)); - map[r][c].auxWeight=dist; - map[r][c].distance=(r==i||c==j)?1:sqrt(2); - sum+=map[r][c].weight; + map[r][c].weight *= dist; + map[r][c].auxWeight = dist; + map[r][c].distance = (r==i||c==j)?1:sqrt(2); } } } } - // map[i][j].weight = map[i][j].weight*gaussian((hit?0:M_PI), (hit?stdev_hit:stdev_miss)); - map[i][j].weight = 0; - // closedPropagationSet.insert(std::pair(i,j)); + map[i][j].weight = map[i][j].weight* gaussian((hit?0:M_PI), (hit?stdev_hit:stdev_miss)); + // map[i][j].weight = 0; + closedPropagationSet.insert(std::pair(i,j)); map[i][j].auxWeight=0; map[i][j].distance=0; - //propagate these short-range estimations to the entire environment using the navigation map + //propagate these estimations to the entire environment propagateProbabilities(map, openPropagationSet, closedPropagationSet, activePropagationSet); } @@ -260,7 +272,7 @@ void InfotaxisGSL::propagateProbabilities(std::vector >& map, std::unordered_set, boost::hash< std::pair > >& activePropagationSet){ while(!activePropagationSet.empty()) { - while(!activePropagationSet.empty()){ + while(!activePropagationSet.empty()) { auto p = *activePropagationSet.begin(); activePropagationSet.erase(activePropagationSet.begin()); closedPropagationSet.insert(p); @@ -272,17 +284,17 @@ void InfotaxisGSL::propagateProbabilities(std::vector >& map, int fC=std::min((int) map[0].size()-1,p.second+1); //8-neighbour propagation - for(int i=oR;i<=fR;i++){ - for(int j=oC;j<=fC;j++){ + for(int i=oR; i<=fR; i++){ + for(int j=oC; j<=fC; j++){ calculateWeight(map, i,j, p, openPropagationSet,closedPropagationSet,activePropagationSet); } } } for(auto& par : openPropagationSet){ - map[par.first][par.second].weight=map[par.first][par.second].weight*map[par.first][par.second].auxWeight; + map[par.first][par.second].weight *= map[par.first][par.second].auxWeight; } - activePropagationSet=openPropagationSet; + activePropagationSet = openPropagationSet; openPropagationSet.clear(); } normalizeWeights(map); @@ -294,21 +306,24 @@ void InfotaxisGSL::setGoal() { int i,j; showWeights(); updateSets(); + std::vector wind = estimateWind(); double ent = -100; double entAux = 0; if (planning_mode == 0) { if(!openMoveSet.empty()) { - for (auto &p:wind) { + for (auto &p:wind) { // loop int r=p.i, c=p.j; entAux = entropy(r,c, Eigen::Vector2d(p.speed,p.angle)); if(entAux > ent){ ent = entAux; - i=r; j=c; + i=r; + j=c; } } - }else { + } + else { ROS_ERROR("Set of open nodes is empty!!!!"); } @@ -341,7 +356,7 @@ void InfotaxisGSL::setGoal() { } openMoveSet.erase(std::pair(i,j)); } - switch_notify(); + switch_notify(planning_mode); planning_mode = 0; //switching back to infotaxis moveTo(i,j); } @@ -355,7 +370,7 @@ void InfotaxisGSL::cancel_navigation() { wind_spd_vector.clear(); wind_dir_vector.clear(); time_stopped = ros::Time::now(); //Start timer for initial wind measurement - currentPosIndex=coordinatesToIndex(current_robot_pose.pose.pose.position.x, current_robot_pose.pose.pose.position.y); + currentPosIndex = coordinatesToIndex(current_pose.pose.pose.position.x, current_pose.pose.pose.position.y); previous_state = current_state; current_state=Infotaxis_state::STOP_AND_MEASURE; } @@ -364,9 +379,9 @@ void InfotaxisGSL::cancel_navigation() { void InfotaxisGSL::updateSets() { int i = currentPosIndex.x(); int j = currentPosIndex.y(); - ROS_ERROR("ENTROPY_GAIN: %f", get_average_vector(entropy_gain_rate)); + // ROS_ERROR("ENTROPY_GAIN: %f", get_average_vector(entropy_gain_rate)); - if (number_revisited > 0){ + if (number_revisited > 0) { ros::Duration time_spent = ros::Time::now() - last_revisited; if (time_spent.toSec() > 60.0) { number_revisited = 0; @@ -388,6 +403,7 @@ void InfotaxisGSL::updateSets() { ROS_WARN("SWITCHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH!!!"); } } + visitedSet.insert(std::pair(i,j)); // check wether pos is near the boundary @@ -397,7 +413,6 @@ void InfotaxisGSL::updateSets() { int fJ = std::min((int) cells[0].size()-1, j+1); openMoveSet.clear(); - for(int r=oI; r<=fI; r++){ for(int c=oJ; c<=fJ; c++){ // if(r==i && c==j){ @@ -412,8 +427,11 @@ void InfotaxisGSL::updateSets() { } double InfotaxisGSL::entropy(int i, int j, Eigen::Vector2d wind) { - auto cells2 = cells; //temp copy of the matrix of cells that we can modify to simulate the effect of a measurement + //copy cells to estimate the probability ,aps without changing it + auto cells2 = cells; double entH = 0; + double entM = 0; + if(wind.x() >= th_wind_present) { estimateProbabilities(cells2, true, wind.y(), Eigen::Vector2i(i,j)); for(int r=0; r const &v) { return average_angle; } +//ask the gmrf_wind service to estimate the wind vector of cells in openMoveSet std::vector InfotaxisGSL::estimateWind(){ - //ask the gmrf_wind service for the estimated wind vector in cell i,j gmrf_wind_mapping::WindEstimation srv; + std::vector> indices; - std::vector > indices; + // Get coordinate of each cells in openMoveSet to call in GMRF_wind service for(auto& p: openMoveSet){ Eigen::Vector2d coords = indexToCoordinates(p.first,p.second); srv.request.x.push_back(coords.x()); @@ -510,13 +527,14 @@ std::vector InfotaxisGSL::estimateWind(){ indices.push_back(p); } + // Call in GMRF_wind service and get result for cells in openMoveSet std::vector result(openMoveSet.size()); if(clientW.call(srv)){ for(int ind=0; ind InfotaxisGSL::estimateWind(){ void InfotaxisGSL::calculateWeight(std::vector >& map, int i, int j, std::pair p, std::unordered_set, boost::hash< std::pair > >& openPropagationSet, std::unordered_set, boost::hash< std::pair > >& closedPropagationSet, - std::unordered_set, boost::hash< std::pair > >& activePropagationSet){ + std::unordered_set, boost::hash< std::pair > >& activePropagationSet) { + if(map[i][j].free && closedPropagationSet.find(std::pair(i,j)) == closedPropagationSet.end() && activePropagationSet.find(std::pair(i,j)) == activePropagationSet.end()){ + //if there already was a path to this cell if(openPropagationSet.find(std::pair(i,j))!= openPropagationSet.end()) { - //if there already was a path to this cell double d = map[p.first][p.second].distance + ((i==p.first||j==p.second)?1:sqrt(2)); //distance of this new path to the same cell - + // ROS_INFO("rghrgrhrjhyjkuli;l %f,%f",map[p.first][p.second].distance,((i==p.first||j==p.second)?1:sqrt(2))); if(abs(d-map[i][j].distance)<0.1){ //if the distance is the same, keep the best probability! map[i][j].auxWeight=std::max(map[p.first][p.second].auxWeight , map[i][j].auxWeight); - - }else if(d(i,j)); } @@ -551,143 +572,41 @@ void InfotaxisGSL::calculateWeight(std::vector >& map, int i, void InfotaxisGSL::normalizeWeights(std::vector >& map) { double s = 0.0; for(int i=0; i((val-low)/(range),0); - b=1; - } - else if(val((val-(low+range))/(range),0); - } - else if(val(0,(val-(low+3*range))/(range)); - b=0; - } - return Eigen::Vector3d(r,g,b); -} - -void InfotaxisGSL::switch_notify() { - double r=0, g=1, b=0; //green - if (planning_mode == 1) { - r=1; g=0; b=0; //red - } - visualization_msgs::Marker marker; - marker.header.frame_id="map"; - marker.header.stamp=ros::Time::now(); - marker.id = 0; - marker.type = visualization_msgs::Marker::SPHERE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = 5.5; - marker.pose.position.y = 7; - marker.pose.position.z = 2.5; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.color.r = r; - marker.color.g = g; - marker.color.b = b; - marker.color.a = 1.0; - marker.scale.x = 0.5; - marker.scale.y = 0.5; - marker.scale.z = 0.5; - switch_marker.publish( marker ); -} - -void InfotaxisGSL::hit_notify() { - double r=0, g=1, b=0; //green - if (gasHit == true) { - r=1; g=0; b=0; //red - } - visualization_msgs::Marker marker; - marker.header.frame_id="map"; - marker.header.stamp=ros::Time::now(); - marker.id = 0; - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = 3.5; - marker.pose.position.y = 7; - marker.pose.position.z = 2.5; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.color.r = r; - marker.color.g = g; - marker.color.b = b; - marker.color.a = 1.0; - marker.scale.x = 0.5; - marker.scale.y = 0.5; - marker.scale.z = 0.5; - hit_marker.publish(marker); } \ No newline at end of file diff --git a/src/visual_cpt.cpp b/src/visual_cpt.cpp new file mode 100644 index 0000000..d058e4f --- /dev/null +++ b/src/visual_cpt.cpp @@ -0,0 +1,137 @@ +#include +#include + +VisualCPT::VisualCPT(ros::NodeHandle *nh): nh_(nh) { + switch_marker = nh_->advertise("switch_marker", 10); + hit_marker = nh_->advertise("hit_marker", 10); + test_marker = nh_->advertise("test_marker", 10); +} + +VisualCPT::~VisualCPT(){} + +visualization_msgs::Marker VisualCPT::emptyMarker(int numCells) { + visualization_msgs::Marker points; + points.header.frame_id="map"; + points.header.stamp=ros::Time::now(); + points.ns = "cells"; + points.id = 0; + points.type=visualization_msgs::Marker::POINTS; + points.action=visualization_msgs::Marker::ADD; + + Eigen::Vector3d colour = valueToColor(1.0/numCells, 0, 1); + points.color.r = colour[0]; + points.color.g = colour[1]; + points.color.b = colour[2]; + points.color.a = 1.0; + points.scale.x=0.15; + points.scale.y=0.15; + return points; +} + + +Eigen::Vector3d VisualCPT::valueToColor(double val, double low, double high){ + double r, g, b; + val = log10(val); + double range=(log10(high)-log10(low))/4; + low=log10(low); + if(val((val-low)/(range),0); + b=1; + } + else if(val((val-(low+range))/(range),0); + } + else if(val(0,(val-(low+3*range))/(range)); + b=0; + } + return Eigen::Vector3d(r,g,b); +} + +void VisualCPT::switch_notify(int planning_mode) { + double r=0, g=1, b=0; //green + if (planning_mode == 1) { + r=1; g=0; b=0; //red + } + visualization_msgs::Marker marker; + marker.header.frame_id="map"; + marker.header.stamp=ros::Time::now(); + marker.id = 0; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = 5.5; + marker.pose.position.y = 7; + marker.pose.position.z = 2.5; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color.r = r; + marker.color.g = g; + marker.color.b = b; + marker.color.a = 1.0; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.scale.z = 0.5; + switch_marker.publish(marker); +} + +void VisualCPT::hit_notify(bool gasHit) { + double r=0, g=1, b=0; //green + if (gasHit == true) { + r=1; g=0; b=0; //red + } + visualization_msgs::Marker marker; + marker.header.frame_id="map"; + marker.header.stamp=ros::Time::now(); + marker.id = 0; + marker.type = visualization_msgs::Marker::CUBE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = 3.5; + marker.pose.position.y = 7; + marker.pose.position.z = 2.5; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color.r = r; + marker.color.g = g; + marker.color.b = b; + marker.color.a = 1.0; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.scale.z = 0.5; + hit_marker.publish(marker); +} + +void VisualCPT::plotplot(float haha) { + visualization_msgs::Marker wind_point_inv; + wind_point_inv.header.frame_id = "map"; + wind_point_inv.id = 1; + wind_point_inv.action = visualization_msgs::Marker::ADD; + wind_point_inv.type = visualization_msgs::Marker::ARROW; + wind_point_inv.header.stamp = ros::Time::now(); + + wind_point_inv.pose.position.x = 0.0; + wind_point_inv.pose.position.y = 0.0; + wind_point_inv.pose.position.z = 0.0; + + wind_point_inv.pose.orientation = tf::createQuaternionMsgFromYaw(haha); + wind_point_inv.scale.x = 2.5; //arrow leng`ht + wind_point_inv.scale.y = 0.1; //arrow width + wind_point_inv.scale.z = 0.1; //arrow height + wind_point_inv.color.r = 0.0; + wind_point_inv.color.g = 0.0; + wind_point_inv.color.b = 1.0; + wind_point_inv.color.a = 1.0; + + test_marker.publish(wind_point_inv); +} \ No newline at end of file