From dae710cc10a8d787cf9f498ef8ddfde646c2806a Mon Sep 17 00:00:00 2001 From: Luong Duc Nhat Date: Thu, 26 May 2022 10:21:44 +0900 Subject: [PATCH 1/9] saved --- src/gsl_infotaxis.cpp | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/gsl_infotaxis.cpp b/src/gsl_infotaxis.cpp index d2b16a5..c1bc3dd 100755 --- a/src/gsl_infotaxis.cpp +++ b/src/gsl_infotaxis.cpp @@ -125,7 +125,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); //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 +133,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()); @@ -201,6 +201,7 @@ void InfotaxisGSL::getGasWindObservations() { } } +//theo nhu code thi wind direction o day la down void InfotaxisGSL::estimateProbabilities(std::vector >& map, bool hit, double wind_direction, Eigen::Vector2i robot_pos) { std::unordered_set< std::pair, boost::hash > > openPropagationSet; std::unordered_set< std::pair, boost::hash > > activePropagationSet; @@ -300,15 +301,17 @@ void InfotaxisGSL::setGoal() { 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!!!!"); } @@ -414,6 +417,8 @@ 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 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 +515,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 Date: Tue, 31 May 2022 11:27:57 +0900 Subject: [PATCH 2/9] adding a marker to check upwind direction --- include/gsl_infotaxis.h | 9 +++++++++ src/gsl_infotaxis.cpp | 43 ++++++++++++++++++++++++++++++++++++++--- 2 files changed, 49 insertions(+), 3 deletions(-) diff --git a/include/gsl_infotaxis.h b/include/gsl_infotaxis.h index a9b1061..fd03a1e 100755 --- a/include/gsl_infotaxis.h +++ b/include/gsl_infotaxis.h @@ -114,6 +114,15 @@ class InfotaxisGSL:public GSLAlgorithm { ros::Publisher hit_marker; void switch_notify(); void hit_notify(); + + + + + ros::Publisher test_marker; + void plotplot(float haha); + + + Eigen::Vector2i currentPosIndex; //Auxiliary functions diff --git a/src/gsl_infotaxis.cpp b/src/gsl_infotaxis.cpp index c1bc3dd..8112986 100755 --- a/src/gsl_infotaxis.cpp +++ b/src/gsl_infotaxis.cpp @@ -32,6 +32,12 @@ InfotaxisGSL::InfotaxisGSL(ros::NodeHandle *nh) : GSLAlgorithm(nh) { hit_marker = nh->advertise("hit_marker", 10); entropy_reporter = nh->advertise("entropy_reporter", 10); + + + test_marker = nh->advertise("test_marker", 10); + + + // Init State gasHit = false; number_revisited = 0; @@ -125,7 +131,8 @@ 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); //simulation wind + 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; @@ -219,6 +226,10 @@ void InfotaxisGSL::estimateProbabilities(std::vector >& map, b 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; + + plotplot(move_dir); + + double maxHit = gaussian(0,stdev_hit); double maxMiss = gaussian(0,stdev_miss); @@ -483,8 +494,7 @@ Eigen::Vector2d InfotaxisGSL::indexToCoordinates(double i, double j){ } double InfotaxisGSL::gaussian(double distance, double sigma){ - return exp(-0.5*(pow(distance,2)/pow(sigma,2)) ) - /(sigma*sqrt(2*M_PI)); + return exp(-0.5*(pow(distance,2)/pow(sigma,2)))/(sigma*sqrt(2*M_PI)); } Infotaxis_state InfotaxisGSL::getState(){ @@ -696,4 +706,31 @@ void InfotaxisGSL::hit_notify() { marker.scale.y = 0.5; marker.scale.z = 0.5; hit_marker.publish(marker); +} + + +void InfotaxisGSL::plotplot(float haha) { + ROS_ERROR("bjasbfjkadvbjkldanvlkjdmnv"); + + 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 From 4df919f5f078a7e03514c6ce54344be77da156f9 Mon Sep 17 00:00:00 2001 From: Luong Duc Nhat Date: Tue, 31 May 2022 17:55:47 +0900 Subject: [PATCH 3/9] fix check next goal by changing to the right name of movebase service --- include/gsl_infotaxis.h | 9 +++------ src/gsl_algorithm.cpp | 25 +++++++++++++++---------- src/gsl_infotaxis.cpp | 17 +++++++---------- 3 files changed, 25 insertions(+), 26 deletions(-) diff --git a/include/gsl_infotaxis.h b/include/gsl_infotaxis.h index fd03a1e..39dc578 100755 --- a/include/gsl_infotaxis.h +++ b/include/gsl_infotaxis.h @@ -17,15 +17,12 @@ 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 { diff --git a/src/gsl_algorithm.cpp b/src/gsl_algorithm.cpp index 72ff75a..deb833f 100755 --- a/src/gsl_algorithm.cpp +++ b/src/gsl_algorithm.cpp @@ -36,7 +36,7 @@ GSLAlgorithm::GSLAlgorithm(ros::NodeHandle *nh) : nh_(nh), mb_ac("move_base", tr 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; } @@ -87,7 +87,11 @@ float GSLAlgorithm::get_average_vector(std::vector const &v) { 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); + 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); //1. Get dimensions of OccupancyMap float map_min_x = map_.info.origin.position.x; @@ -96,29 +100,30 @@ bool GSLAlgorithm::checkGoal(move_base_msgs::MoveBaseGoal * goal) { 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 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; + 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. + //get path from robot to next target. + mb_client.call(mb_srv); 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; } } diff --git a/src/gsl_infotaxis.cpp b/src/gsl_infotaxis.cpp index 8112986..cf38494 100755 --- a/src/gsl_infotaxis.cpp +++ b/src/gsl_infotaxis.cpp @@ -3,12 +3,12 @@ #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) { @@ -31,9 +31,6 @@ InfotaxisGSL::InfotaxisGSL(ros::NodeHandle *nh) : GSLAlgorithm(nh) { switch_marker = nh->advertise("switch_marker", 10); hit_marker = nh->advertise("hit_marker", 10); entropy_reporter = nh->advertise("entropy_reporter", 10); - - - test_marker = nh->advertise("test_marker", 10); @@ -465,7 +462,6 @@ void InfotaxisGSL::moveTo(int i, int j) { goal.target_pose.pose.position.x = coordR.x(); goal.target_pose.pose.position.y = coordR.y(); goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(angles::normalize_angle(move_angle)); - ROS_INFO("MOVING TO %f,%f",pos.x(),pos.y()); mb_ac.sendGoal(goal, boost::bind(&InfotaxisGSL::goalDoneCallback, this, _1, _2), boost::bind(&InfotaxisGSL::goalActiveCallback, this), boost::bind(&InfotaxisGSL::goalFeedbackCallback, this, _1)); @@ -475,6 +471,7 @@ void InfotaxisGSL::moveTo(int i, int j) { goal.target_pose.pose.position.x = pos.x(); goal.target_pose.pose.position.y = pos.y(); goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(angles::normalize_angle(move_angle)); + while(!checkGoal(&goal)); mb_ac.sendGoal(goal, boost::bind(&InfotaxisGSL::goalDoneCallback, this, _1, _2), boost::bind(&InfotaxisGSL::goalActiveCallback, this), boost::bind(&InfotaxisGSL::goalFeedbackCallback, this, _1)); inMotion=true; From 9ee84f5481e8bf2bc260bfb0f23c9f593ffcd594 Mon Sep 17 00:00:00 2001 From: Luong Duc Nhat Date: Tue, 31 May 2022 17:58:57 +0900 Subject: [PATCH 4/9] make shorter lines --- src/gsl_algorithm.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/gsl_algorithm.cpp b/src/gsl_algorithm.cpp index deb833f..f960b9b 100755 --- a/src/gsl_algorithm.cpp +++ b/src/gsl_algorithm.cpp @@ -86,26 +86,24 @@ 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) { 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); - //1. Get dimensions of OccupancyMap + // 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 (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; @@ -117,7 +115,6 @@ bool GSLAlgorithm::checkGoal(move_base_msgs::MoveBaseGoal * goal) { mb_srv.request.goal = target; mb_srv.request.tolerance = 0.1; - //get path from robot to next target. mb_client.call(mb_srv); if( mb_client.call(mb_srv) && mb_srv.response.plan.poses.size()>1) { return true; @@ -134,7 +131,7 @@ int GSLAlgorithm::checkSourceFound() { //Check if timeout ros::Duration time_spent = ros::Time::now() - start_time; if (time_spent.toSec() > max_search_time) { - ROS_INFO("FAILURE-> Time spent (%.3f s) > max_search_time = %.3f", time_spent.toSec(), max_search_time); + ROS_INFO("FAILURE-> Time spent (%.3f s) > time_limit = %.3f", time_spent.toSec(), max_search_time); return 0; } From af5b2ff7a158f922249a1e46902a5bf26bb7d199 Mon Sep 17 00:00:00 2001 From: Luong Duc Nhat Date: Wed, 1 Jun 2022 17:28:08 +0900 Subject: [PATCH 5/9] fix bug in angle calculation in cell direction --- include/gsl_algorithm.h | 2 +- include/gsl_infotaxis.h | 5 +- src/gsl_algorithm.cpp | 10 +-- src/gsl_infotaxis.cpp | 177 +++++++++++++++++++++++++++------------- 4 files changed, 130 insertions(+), 64 deletions(-) diff --git a/include/gsl_algorithm.h b/include/gsl_algorithm.h index 44e3430..5a5f0d2 100755 --- a/include/gsl_algorithm.h +++ b/include/gsl_algorithm.h @@ -44,7 +44,7 @@ class GSLAlgorithm { 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 39dc578..6f8e7c1 100755 --- a/include/gsl_infotaxis.h +++ b/include/gsl_infotaxis.h @@ -64,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, diff --git a/src/gsl_algorithm.cpp b/src/gsl_algorithm.cpp index f960b9b..5212033 100755 --- a/src/gsl_algorithm.cpp +++ b/src/gsl_algorithm.cpp @@ -49,8 +49,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 } @@ -109,7 +109,7 @@ bool GSLAlgorithm::checkGoal(move_base_msgs::MoveBaseGoal * goal) { 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.goal = target; @@ -136,8 +136,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 cf38494..aafd5c7 100755 --- a/src/gsl_infotaxis.cpp +++ b/src/gsl_infotaxis.cpp @@ -14,7 +14,7 @@ Cell::Cell(bool f, double a, double b, double c) { InfotaxisGSL::InfotaxisGSL(ros::NodeHandle *nh) : GSLAlgorithm(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.5); 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? @@ -31,9 +31,7 @@ InfotaxisGSL::InfotaxisGSL(ros::NodeHandle *nh) : GSLAlgorithm(nh) { switch_marker = nh->advertise("switch_marker", 10); hit_marker = nh->advertise("hit_marker", 10); entropy_reporter = nh->advertise("entropy_reporter", 10); - test_marker = nh->advertise("test_marker", 10); - - + test_marker = nh->advertise("test_marker", 10); // Init State gasHit = false; @@ -56,34 +54,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); } @@ -129,7 +134,6 @@ 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 + 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; @@ -173,81 +177,87 @@ 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(); + // 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(); + // 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(); estimateProbabilities(cells, gasHit, avg_wind_dir, currentPosIndex); previous_state=current_state; current_state = Infotaxis_state::MOVING; - ROS_WARN("NOTHING!!!! New state --> MOVING"); + } - hit_notify(); } } +/* //theo nhu code thi wind direction o day la down void InfotaxisGSL::estimateProbabilities(std::vector >& map, bool hit, double wind_direction, Eigen::Vector2i robot_pos) { std::unordered_set< std::pair, boost::hash > > openPropagationSet; 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 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; - plotplot(move_dir); - - double maxHit = gaussian(0,stdev_hit); double maxMiss = gaussian(0,stdev_miss); + + for(int r=oI; r<=fI; r++) { for(int c=oJ; c<=fJ; c++) { if(map[r][c].free) { 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((coordR.y()-coordP.y()),(coordR.x()-coordP.x()))); + ROS_ERROR("NHATTTT: %f, %f, %f, %f, %f", coordR.x(), coordR.y(), coordP.x(), coordP.y(), cell_vector); if(hit) { dist=gaussian(atan2(sin(upwind_dir-cell_vector), cos(upwind_dir-cell_vector)),stdev_hit); - }else { + // ROS_ERROR("ANGLEEEEEE: %f, %f, %f, %f, %f", wind_direction, upwind_dir, cell_vector, dist, map[r][c].weight); + } + 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].weight; + ROS_ERROR("DMMM, %f", map[r][c].weight); + map[r][c].auxWeight = dist; + map[r][c].distance = (r==i||c==j)?1:sqrt(2); } } } @@ -258,6 +268,64 @@ void InfotaxisGSL::estimateProbabilities(std::vector >& map, b // closedPropagationSet.insert(std::pair(i,j)); map[i][j].auxWeight=0; map[i][j].distance=0; + showWeights(); + //propagate these short-range estimations to the entire environment using the navigation map + // propagateProbabilities(map, openPropagationSet, closedPropagationSet, activePropagationSet); +} */ + + + + +void InfotaxisGSL::estimateProbabilities(std::vector >& map, bool hit, double wind_direction, Eigen::Vector2i robot_pos) { + std::unordered_set< std::pair, boost::hash > > openPropagationSet; + std::unordered_set< std::pair, boost::hash > > activePropagationSet; + std::unordered_set< std::pair, boost::hash > > closedPropagationSet; + + 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); + + //estimate the probabilities for the immediate 8 neighbours + Eigen::Vector2d coordR = indexToCoordinates(i,j); + 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); + + for(int r=oI; r<=fI; r++) { + for(int c=oJ; c<=fJ; c++) { + if(map[r][c].free) { + if(c!=j || r!=i) { + Eigen::Vector2d coordP = indexToCoordinates(r,c); + double dist; + 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 { + dist=gaussian(atan2(sin(move_dir-cell_vector), cos(move_dir-cell_vector)), stdev_miss); + } + + activePropagationSet.insert(std::pair(r,c)); + map[r][c].weight = dist*map[r][c].weight; + 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].auxWeight=0; + map[i][j].distance=0; //propagate these short-range estimations to the entire environment using the navigation map propagateProbabilities(map, openPropagationSet, closedPropagationSet, activePropagationSet); @@ -366,7 +434,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; } @@ -423,7 +491,8 @@ 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; @@ -482,7 +551,7 @@ void InfotaxisGSL::moveTo(int i, int j) { Eigen::Vector2i InfotaxisGSL::coordinatesToIndex(double x, double y){ return Eigen::Vector2i((y-map_.info.origin.position.y)/(scale*map_.info.resolution), - (x-map_.info.origin.position.x)/(scale*map_.info.resolution)); + (x-map_.info.origin.position.x)/(scale*map_.info.resolution)); } Eigen::Vector2d InfotaxisGSL::indexToCoordinates(double i, double j){ @@ -564,14 +633,14 @@ void InfotaxisGSL::calculateWeight(std::vector >& map, int i, void InfotaxisGSL::normalizeWeights(std::vector >& map) { double s = 0.0; for(int i=0; i Date: Wed, 1 Jun 2022 17:30:57 +0900 Subject: [PATCH 6/9] delete param of robot_initial_pose --- include/gsl_algorithm.h | 1 - src/gsl_algorithm.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/include/gsl_algorithm.h b/include/gsl_algorithm.h index 5a5f0d2..e0fa4b9 100755 --- a/include/gsl_algorithm.h +++ b/include/gsl_algorithm.h @@ -39,7 +39,6 @@ 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 diff --git a/src/gsl_algorithm.cpp b/src/gsl_algorithm.cpp index 5212033..0c32d47 100755 --- a/src/gsl_algorithm.cpp +++ b/src/gsl_algorithm.cpp @@ -29,8 +29,6 @@ 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); From b0a60e11cfabd423bafdf3037b2f5139d3cab2de Mon Sep 17 00:00:00 2001 From: Luong Duc Nhat Date: Thu, 2 Jun 2022 11:16:32 +0900 Subject: [PATCH 7/9] make a seperate class for visualization --- CMakeLists.txt | 2 +- include/gsl_infotaxis.h | 21 +--- include/visual_cpt.h | 24 +++++ package.xml | 2 +- src/gsl_infotaxis.cpp | 209 ++-------------------------------------- src/visual_cpt.cpp | 137 ++++++++++++++++++++++++++ 6 files changed, 175 insertions(+), 220 deletions(-) create mode 100644 include/visual_cpt.h create mode 100644 src/visual_cpt.cpp 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_infotaxis.h b/include/gsl_infotaxis.h index 6f8e7c1..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}; @@ -25,7 +25,7 @@ struct WindVector { double speed, angle; }; -class InfotaxisGSL:public GSLAlgorithm { +class InfotaxisGSL:public GSLAlgorithm, public VisualCPT { public: InfotaxisGSL(ros::NodeHandle *nh); ~InfotaxisGSL(); @@ -106,25 +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(); - - - - - ros::Publisher test_marker; - void plotplot(float haha); - - - + 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_infotaxis.cpp b/src/gsl_infotaxis.cpp index aafd5c7..eb7c9df 100755 --- a/src/gsl_infotaxis.cpp +++ b/src/gsl_infotaxis.cpp @@ -11,10 +11,10 @@ Cell::Cell(bool f, double a, double b, double c) { 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, 2.5); + 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,7 @@ 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); - test_marker = nh->advertise("test_marker", 10); // Init State gasHit = false; @@ -179,13 +176,13 @@ void InfotaxisGSL::getGasWindObservations() { wind_spd_vector.clear(); previous_robot_pose=Eigen::Vector2d(current_pose.pose.pose.position.x, current_pose.pose.pose.position.y); previous_state = current_state; - + // plotplot(avg_wind_dir); 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; - hit_notify(); + 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; @@ -194,7 +191,7 @@ void InfotaxisGSL::getGasWindObservations() { //Only gas ROS_WARN("GAS, BUT NO WIND New state --> MOVING"); gasHit=true; - hit_notify(); + 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; @@ -204,7 +201,7 @@ void InfotaxisGSL::getGasWindObservations() { //Nothing gasHit=false; ROS_WARN("NOTHING!!!! New state --> MOVING"); - hit_notify(); + hit_notify(gasHit); estimateProbabilities(cells, gasHit, avg_wind_dir, currentPosIndex); previous_state=current_state; current_state = Infotaxis_state::MOVING; @@ -213,69 +210,6 @@ void InfotaxisGSL::getGasWindObservations() { } } -/* -//theo nhu code thi wind direction o day la down -void InfotaxisGSL::estimateProbabilities(std::vector >& map, bool hit, double wind_direction, Eigen::Vector2i robot_pos) { - std::unordered_set< std::pair, boost::hash > > openPropagationSet; - std::unordered_set< std::pair, boost::hash > > activePropagationSet; - std::unordered_set< std::pair, boost::hash > > closedPropagationSet; - 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); - - //estimate the probabilities for the immediate 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 maxHit = gaussian(0,stdev_hit); - double maxMiss = gaussian(0,stdev_miss); - - - - for(int r=oI; r<=fI; r++) { - for(int c=oJ; c<=fJ; c++) { - if(map[r][c].free) { - if(c!=j || r!=i) { - Eigen::Vector2d coordP = indexToCoordinates(r,c); - double dist; - double cell_vector = angles::normalize_angle(atan2((coordR.y()-coordP.y()),(coordR.x()-coordP.x()))); - ROS_ERROR("NHATTTT: %f, %f, %f, %f, %f", coordR.x(), coordR.y(), coordP.x(), coordP.y(), cell_vector); - - if(hit) { - dist=gaussian(atan2(sin(upwind_dir-cell_vector), cos(upwind_dir-cell_vector)),stdev_hit); - // ROS_ERROR("ANGLEEEEEE: %f, %f, %f, %f, %f", wind_direction, upwind_dir, cell_vector, dist, map[r][c].weight); - } - else { - dist=gaussian(atan2(sin(move_dir-cell_vector), cos(move_dir-cell_vector)), stdev_miss); - } - - activePropagationSet.insert(std::pair(r,c)); - map[r][c].weight = dist*map[r][c].weight; - ROS_ERROR("DMMM, %f", map[r][c].weight); - 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].auxWeight=0; - map[i][j].distance=0; - showWeights(); - //propagate these short-range estimations to the entire environment using the navigation map - // propagateProbabilities(map, openPropagationSet, closedPropagationSet, activePropagationSet); -} */ - - - - void InfotaxisGSL::estimateProbabilities(std::vector >& map, bool hit, double wind_direction, Eigen::Vector2i robot_pos) { std::unordered_set< std::pair, boost::hash > > openPropagationSet; std::unordered_set< std::pair, boost::hash > > activePropagationSet; @@ -420,7 +354,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); } @@ -650,7 +584,7 @@ void InfotaxisGSL::normalizeWeights(std::vector >& map) { //============================ VISUALIZATION =============================== void InfotaxisGSL::showWeights() { - visualization_msgs::Marker points = emptyMarker(); + visualization_msgs::Marker points = emptyMarker(numCells); for(int a=0; a((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); -} - - -void InfotaxisGSL::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 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 From fd0534ce2566b424e07f7c8a0370d33aecf3cfeb Mon Sep 17 00:00:00 2001 From: Luong Duc Nhat Date: Wed, 29 Jun 2022 11:48:48 +0900 Subject: [PATCH 8/9] change some comment to match the real_world branch --- src/gsl_algorithm.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/gsl_algorithm.cpp b/src/gsl_algorithm.cpp index 0c32d47..8820ec5 100755 --- a/src/gsl_algorithm.cpp +++ b/src/gsl_algorithm.cpp @@ -90,7 +90,6 @@ bool GSLAlgorithm::checkGoal(move_base_msgs::MoveBaseGoal * goal) { 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; @@ -104,7 +103,6 @@ bool GSLAlgorithm::checkGoal(move_base_msgs::MoveBaseGoal * 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_pose.pose.pose; @@ -113,7 +111,7 @@ bool GSLAlgorithm::checkGoal(move_base_msgs::MoveBaseGoal * goal) { mb_srv.request.goal = target; mb_srv.request.tolerance = 0.1; - mb_client.call(mb_srv); + //get path from robot to candidate. if( mb_client.call(mb_srv) && mb_srv.response.plan.poses.size()>1) { return true; } @@ -129,7 +127,7 @@ int GSLAlgorithm::checkSourceFound() { //Check if timeout ros::Duration time_spent = ros::Time::now() - start_time; if (time_spent.toSec() > max_search_time) { - ROS_INFO("FAILURE-> Time spent (%.3f s) > time_limit = %.3f", time_spent.toSec(), max_search_time); + ROS_INFO("FAILURE-> Time spent (%.3f s) > max_search_time = %.3f", time_spent.toSec(), max_search_time); return 0; } From 5b31c1858458550c3dd917cca38209223367f8fc Mon Sep 17 00:00:00 2001 From: Luong Duc Nhat Date: Wed, 29 Jun 2022 18:40:35 +0900 Subject: [PATCH 9/9] change format --- src/gsl_infotaxis.cpp | 63 +++++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 29 deletions(-) diff --git a/src/gsl_infotaxis.cpp b/src/gsl_infotaxis.cpp index eb7c9df..663f428 100755 --- a/src/gsl_infotaxis.cpp +++ b/src/gsl_infotaxis.cpp @@ -176,8 +176,9 @@ void InfotaxisGSL::getGasWindObservations() { wind_spd_vector.clear(); previous_robot_pose=Eigen::Vector2d(current_pose.pose.pose.position.x, current_pose.pose.pose.position.y); previous_state = current_state; - // plotplot(avg_wind_dir); + 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"); @@ -222,9 +223,9 @@ void InfotaxisGSL::estimateProbabilities(std::vector >& map, b int oJ = std::max(0,j-1); int fJ = std::min((int)map[0].size()-1, j+1); - //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 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) @@ -247,7 +248,7 @@ void InfotaxisGSL::estimateProbabilities(std::vector >& map, b } activePropagationSet.insert(std::pair(r,c)); - map[r][c].weight = dist*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); } @@ -255,13 +256,13 @@ void InfotaxisGSL::estimateProbabilities(std::vector >& map, b } } - map[i][j].weight = map[i][j].weight*gaussian((hit?0:M_PI), (hit?stdev_hit:stdev_miss)); + 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)); + 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); } @@ -271,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); @@ -283,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); @@ -305,6 +306,7 @@ void InfotaxisGSL::setGoal() { int i,j; showWeights(); updateSets(); + std::vector wind = estimateWind(); double ent = -100; double entAux = 0; @@ -377,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; @@ -401,6 +403,7 @@ void InfotaxisGSL::updateSets() { ROS_WARN("SWITCHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH!!!"); } } + visitedSet.insert(std::pair(i,j)); // check wether pos is near the boundary @@ -410,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){ @@ -543,21 +545,24 @@ std::vector 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)); } @@ -581,8 +586,8 @@ void InfotaxisGSL::normalizeWeights(std::vector >& map) { } } -//============================ VISUALIZATION =============================== +// Visulize the lastest source location probability map void InfotaxisGSL::showWeights() { visualization_msgs::Marker points = emptyMarker(numCells); for(int a=0; a