Simulation

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

include/gsl_algorithm.h

@@ -39,12 +39,11 @@ class GSLAlgorithm {
         double distance_found;
         std::vector<geometry_msgs::PoseWithCovarianceStamped> 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

include/gsl_infotaxis.h

@@ -3,11 +3,11 @@
 #include <iostream>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
-#include <eigen3/Eigen/Dense>
 #include <math.h>
 #include <bits/stdc++.h>
 #include <unordered_set>
 #include <gmrf_wind_mapping/WindEstimation.h>
+#include <visual_cpt.h>
 
 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 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<std::vector<Cell> >& map,
-                                    bool hit,
-                                    double wind_direction,
+        void estimateProbabilities(std::vector<std::vector<Cell> >& map, bool hit, double wind_direction, 
                                     Eigen::Vector2i robot_pos);
+
         void propagateProbabilities(std::vector<std::vector<Cell> >& map,
                                     std::unordered_set<std::pair<int, int>, boost::hash< std::pair<int, int> > >& openSet,
                                     std::unordered_set<std::pair<int, int>, boost::hash< std::pair<int, int> > >& closedSet,
@@ -110,16 +106,12 @@ class InfotaxisGSL:public GSLAlgorithm {
         std::vector<std::vector<Cell> > 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);

include/visual_cpt.h

@@ -0,0 +1,24 @@
+#ifndef VISUAL_CPT
+#define VISUAL_CPT
+#include <ros/ros.h>
+#include <eigen3/Eigen/Dense>
+#include <visualization_msgs/Marker.h>
+
+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

package.xml

@@ -6,7 +6,7 @@
 
   <!-- This package is inspried by Pepe Ojeda; email="ojedamorala@uma.es"-->
 
-  <maintainer email="luong.d.aa@m.titech.ac.jp">Pepe Ojeda</maintainer>
+  <maintainer email="luong.d.aa@m.titech.ac.jp">Nhat Luong</maintainer>
 
   <license>GPLv3</license>
 

src/gsl_algorithm.cpp

@@ -29,14 +29,12 @@ GSLAlgorithm::GSLAlgorithm(ros::NodeHandle *nh) : nh_(nh), mb_ac("move_base", tr
     nh->param<double>("distance_found", distance_found, 0.5);
     nh->param<double>("ground_truth_x", source_pose_x, 1.5);
     nh->param<double>("ground_truth_y", source_pose_y, 3.0);
-    nh->param<double>("robot_pose_x", robot_pose_x, 0.0);
-    nh->param<double>("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<nav_msgs::GetPlan>("/move_base/GlobalPlanner/make_plan");
+    mb_client = nh_->serviceClient<nav_msgs::GetPlan>("/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<float> 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());