OGRID: add size param and visuals

command/sub8_launch/launch/subsystems/blueview.launch

@@ -13,7 +13,7 @@
             period_seconds: -1.0
             range:
               stop: 15
-            range_profile_intensity_threshold: 2000
+            range_profile_intensity_threshold: 0
             noise_threshold: 0.2
         </rosparam>
         <param name="color/map_file" value="$(find mil_blueview_driver)/bvtsdk/colormaps/jet.cmap" />

legacy/c3_trajectory_generator/include/waypoint_validity.hpp

@@ -17,13 +17,24 @@ enum class WAYPOINT_ERROR_TYPE
   OCCUPIED_TRAJECTORY = 98
 };
 
+enum class OGRID_COLOR
+{
+  ORANGE = 200,
+  RED = 130,
+  GREEN = 120
+};
+
 class WaypointValidity
 {
 private:
   ros::NodeHandle *nh_;
   nav_msgs::OccupancyGridConstPtr ogrid_map_;
   ros::Subscriber sub_;
 
+  ros::Publisher pub_sub_ogrid_;
+  ros::Publisher pub_waypoint_ogrid_;
+  double sub_ogrid_size_;
+
   // Usage: Given a size and point, relative to ogrid, will check if there is an occupied grid
   bool check_if_hit(cv::Point center, cv::Size sub_size);
 
@@ -33,6 +44,8 @@ class WaypointValidity
   // Usage: Store the reference to the previous ogrid in publisher
   void ogrid_callback(const nav_msgs::OccupancyGridConstPtr &ogrid_map);
 
+  void pub_size_ogrid(const geometry_msgs::Pose &waypoint, int d = 0);
+
   // Usage: Given a waypoint or trajectory, check what it will hit on the ogrid.
   std::pair<bool, WAYPOINT_ERROR_TYPE> is_waypoint_valid(const geometry_msgs::Pose &waypoint,
                                                          bool do_waypoint_validation);

legacy/c3_trajectory_generator/src/node.cpp

@@ -209,13 +209,16 @@ struct Node
       this->linear_tolerance = goal->linear_tolerance;
       this->angular_tolerance = goal->angular_tolerance;
 
+      waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);
+
       // Check if waypoint is valid
       std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
           Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
       actionresult_.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
       actionresult_.success = checkWPResult.first;
       if (checkWPResult.first == false)  // got a point that we should not move to
       {
+        waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
         if (checkWPResult.second ==
             WAYPOINT_ERROR_TYPE::UNKNOWN)  // if unknown, check if there's a huge displacement with the new waypoint
         {
@@ -296,6 +299,8 @@ struct Node
     msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
     trajectory_pub.publish(msg);
 
+    waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200);
+
     PoseStamped msgVis;
     msgVis.header = msg.header;
     msgVis.pose = msg.posetwist.pose;

legacy/c3_trajectory_generator/src/waypoint_validity.cpp

@@ -45,12 +45,17 @@ std::pair<bool, WAYPOINT_ERROR_TYPE> WaypointValidity::is_waypoint_valid(const g
     return std::make_pair(false, WAYPOINT_ERROR_TYPE::NO_OGRID);
   }
 
-  cv::Point where_sub = cv::Point(waypoint.position.x / ogrid_map_->info.resolution + ogrid_map_->info.width / 2,
-                                  waypoint.position.y / ogrid_map_->info.resolution + ogrid_map_->info.height / 2);
+  cv::Point center_of_ogrid =
+      cv::Point(ogrid_map_->info.origin.position.x, ogrid_map_->info.origin.position.y) +
+      cv::Point(ogrid_map_->info.width, ogrid_map_->info.height) * ogrid_map_->info.resolution / 2;
+  cv::Point where_sub =
+      cv::Point((waypoint.position.x - center_of_ogrid.x) / ogrid_map_->info.resolution + ogrid_map_->info.width / 2,
+                (waypoint.position.y - center_of_ogrid.y) / ogrid_map_->info.resolution + ogrid_map_->info.height / 2);
 
   // Area we want to check around the sub
-  int sub_x = 1.5 / ogrid_map_->info.resolution;
-  int sub_y = 1.5 / ogrid_map_->info.resolution;
+  int sub_x = sub_ogrid_size_ / ogrid_map_->info.resolution;
+  int sub_y = sub_ogrid_size_ / ogrid_map_->info.resolution;
+
   if (check_if_hit(where_sub, cv::Size(sub_x, sub_y)))
   {
     return std::make_pair(false, WAYPOINT_ERROR_TYPE::OCCUPIED);
@@ -70,9 +75,34 @@ std::pair<bool, WAYPOINT_ERROR_TYPE> WaypointValidity::is_waypoint_valid(const g
   return std::make_pair(true, WAYPOINT_ERROR_TYPE::UNOCCUPIED);
 }
 
+void WaypointValidity::pub_size_ogrid(const geometry_msgs::Pose &waypoint, int d)
+{
+  if (!this->ogrid_map_)
+    return;
+  std::vector<int8_t> sub_ogrid_data(std::pow(sub_ogrid_size_ / ogrid_map_->info.resolution, 2), d);
+  nav_msgs::OccupancyGrid rosGrid;
+  rosGrid.header.seq = 0;
+  rosGrid.info.resolution = ogrid_map_->info.resolution;
+  rosGrid.header.frame_id = "map";
+  rosGrid.header.stamp = ros::Time::now();
+  rosGrid.info.map_load_time = ros::Time::now();
+  rosGrid.info.width = sub_ogrid_size_ / ogrid_map_->info.resolution;
+  rosGrid.info.height = sub_ogrid_size_ / ogrid_map_->info.resolution;
+  rosGrid.info.origin.position.x = waypoint.position.x - sub_ogrid_size_ / 2;
+  rosGrid.info.origin.position.y = waypoint.position.y - sub_ogrid_size_ / 2;
+  rosGrid.data = sub_ogrid_data;
+  if (d == (int)OGRID_COLOR::ORANGE)
+    pub_sub_ogrid_.publish(rosGrid);
+  else
+    pub_waypoint_ogrid_.publish(rosGrid);
+}
+
 WaypointValidity::WaypointValidity(ros::NodeHandle &nh)
 {
   nh_ = &nh;
   sub_ = nh_->subscribe<nav_msgs::OccupancyGrid>("/ogrid_pointcloud/ogrid", 1,
                                                  boost::bind(&WaypointValidity::ogrid_callback, this, _1));
+  nh_->param<double>("sub_ogrid_size", sub_ogrid_size_, 1.5);
+  pub_waypoint_ogrid_ = nh_->advertise<nav_msgs::OccupancyGrid>("/c3_trajectory_generator/waypoint_ogrid", 1, true);
+  pub_sub_ogrid_ = nh_->advertise<nav_msgs::OccupancyGrid>("/c3_trajectory_generator/sub_ogrid", 1, true);
 }

perception/sub8_pointcloud/include/Classification.hpp

@@ -33,15 +33,22 @@ class Classification
   // Usage: Given starting point, and an angle, find the first occupied point in that direction.
   cv::Point2d get_first_hit(cv::Mat &mat_ogrid, cv::Point2d start, float theta, int max_dis);
 
+  float certainty_as_hit_;
+  int hit_buffer_;
+  float uncertainty_as_hit_;
+  float not_hit_degrade_;
+
 public:
   Classification(ros::NodeHandle *nh);
 
   // Usage: statistical filterting to get rid of outliers and noise
   pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(pcl::PointCloud<pcl::PointXYZI>::ConstPtr pointCloud);
 
-  // Usage: Find all the first occupied points in an expanding circle, then color the ogrid
-  void zonify(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform);
-
-  // Usage: Draw a fake ogrid to test in SIM
-  void fake_ogrid(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform);
+  /* Usage: Find all the first occupied points in an expanding circle, then color the ogrid
+     param mat_ogrid: what ogrid will be used for processing and drawn on
+     param resolution: used to convert meters to pixels
+     param transform: get subs pose
+     param mat_origin: where the center of the ogrid is in resepct to map frame
+  */
+  void zonify(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform, cv::Point &mat_origin);
 };

perception/sub8_pointcloud/include/OGridGen.hpp

@@ -27,34 +27,65 @@
 
 #include <Classification.hpp>
 
+#include <mil_msgs/RangeStamped.h>
+#include <std_srvs/Trigger.h>
+#include <ros_alarms/listener.hpp>
+
 class OGridGen
+
 {
 public:
   OGridGen();
-  void publish_ogrid(const ros::TimerEvent &);
+  void publish_big_pointcloud(const ros::TimerEvent &);
 
   void callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg);
+  void dvl_callback(const mil_msgs::RangeStampedConstPtr &dvl);
+
+  bool clear_ogrid_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
+
+  // Publish mat_ogrid
+  void publish_ogrid();
+  // Project point_cloud and make a persistant ogrid
+  void process_persistant_ogrid(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_plane);
+  // Convert persistant ogrid to a mat_ogrid
+  void populate_mat_ogrid();
 
 private:
   ros::NodeHandle nh_;
   ros::Subscriber sub_to_imaging_sonar_;
+  ros::Subscriber sub_to_dvl_;
 
   tf::TransformListener listener_;
 
+  ros_alarms::AlarmListener<> kill_listener_;
+  // remember if the sub was killed or not in order to reset ogrid origin
+  bool was_killed_;
+  // where the ogrid is with respect to map frame
+  cv::Point mat_origin_;
+
+  // Publish ogrid and pointclouds
   ros::Publisher pub_grid_;
   ros::Publisher pub_point_cloud_filtered_;
   ros::Publisher pub_point_cloud_raw_;
+  ros::Publisher pub_point_cloud_plane_;
+
+  ros::ServiceServer clear_ogrid_service_;
   ros::Timer timer_;
 
+  // A CV_32F Mat to store probability of occupied/unoccupied spaces
+  cv::Mat persistant_ogrid_;
+  float hit_prob_;
+
   cv::Mat mat_ogrid_;
   float ogrid_size_;
   float resolution_;
-  float pool_depth_;
+  double dvl_range_;
   int min_intensity_;
 
   ros::ServiceClient service_get_bounds_;
   tf::StampedTransform transform_;
 
+  // Storage container for the pointcloud
   boost::circular_buffer<pcl::PointXYZI> point_cloud_buffer_;
 
   std::vector<cv::Point> bounds_;

perception/sub8_pointcloud/launch/ogrid.launch

@@ -7,9 +7,6 @@
             # width and height of ogrid in meters 
             ogrid_size: 150
 
-            # Ignore blueview imaging sonar points below depth in meters
-            pool_depth: 3.0
-
             # How many points should be allowed
             buffer_size: 7000
             min_intensity: 0

perception/sub8_pointcloud/src/Classification.cpp

@@ -4,8 +4,6 @@
 Classification::Classification(ros::NodeHandle *nh)
 {
   nh_ = nh;
-
-  // TODO: Algorithms to filter pointcloud and possibly some classification scheme
 }
 
 pcl::PointCloud<pcl::PointXYZI>::Ptr Classification::filtered(pcl::PointCloud<pcl::PointXYZI>::ConstPtr pointCloud)
@@ -26,54 +24,63 @@ cv::Point2d Classification::get_first_hit(cv::Mat &mat_ogrid, cv::Point2d start,
 {
   cv::Rect rect(cv::Point(0, 0), mat_ogrid.size());
   cv::Point2d vec_d_theta(cos(theta), sin(theta));
+
+  // loop through all points in a ray
   for (int i = 0; i < max_dis; ++i)
   {
     cv::Point2d p_on_ray = vec_d_theta * i + start;
     if (!rect.contains(p_on_ray))
       return start;
-    if (mat_ogrid.at<uchar>(p_on_ray.y, p_on_ray.x) == (uchar)WAYPOINT_ERROR_TYPE::OCCUPIED)
+    // is this an object (occupied region)?
+    if (mat_ogrid.at<float>(p_on_ray.y, p_on_ray.x) > certainty_as_hit_)
     {
+      // mark everything behind the object as unknown
+      for (int j = i + hit_buffer_; j < max_dis; ++j)
+      {
+        cv::Point2d cp_on_ray = vec_d_theta * j + start;
+        if (!rect.contains(cp_on_ray))
+          break;
+        mat_ogrid.at<float>(cp_on_ray.y, cp_on_ray.x) = .5;
+      }
+      // degrade things infront of the object as unoccupied
+      for (int j = 0; j < i; ++j)
+      {
+        cv::Point2d cp_on_ray = vec_d_theta * j + start;
+        if (!rect.contains(cp_on_ray))
+          break;
+        if (mat_ogrid.at<float>(cp_on_ray.y, cp_on_ray.x) > 0 &&
+            mat_ogrid.at<float>(cp_on_ray.y, cp_on_ray.x) < uncertainty_as_hit_)
+        {
+          mat_ogrid.at<float>(cp_on_ray.y, cp_on_ray.x) -= not_hit_degrade_;
+        }
+      }
       return p_on_ray;
     }
   }
 
   return start;
 }
 
-void Classification::fake_ogrid(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform)
-{
-  // Sub's position relative to the ogrid
-  cv::Point where_sub = cv::Point2d(transform.getOrigin().x() / resolution + mat_ogrid.cols / 2,
-                                    transform.getOrigin().y() / resolution + mat_ogrid.rows / 2);
-  cv::rectangle(mat_ogrid, cv::Point(-10, -10) + where_sub, cv::Point(15, 10) + where_sub,
-                cv::Scalar((uchar)WAYPOINT_ERROR_TYPE::OCCUPIED), 2);
-  cv::rectangle(mat_ogrid, cv::Point(-5, -5) + where_sub, cv::Point(12, 5) + where_sub, cv::Scalar(0), -1);
-  cv::rectangle(mat_ogrid, cv::Point(5, -2) + where_sub, cv::Point(7, 0) + where_sub,
-                cv::Scalar((uchar)WAYPOINT_ERROR_TYPE::OCCUPIED), -1);
-}
-
 /*
   Get first incident points in rays that are generated by changing angles and forming a circle.
   Then draw a filled polygon using those incident points
 */
-void Classification::zonify(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform)
+void Classification::zonify(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform,
+                            cv::Point &mat_origin)
 {
+  // Runtime debugging
+  nh_->param<float>("/ogrid_pointcloud/certainty_as_hit", certainty_as_hit_, 0.95);
+  nh_->param<int>("/ogrid_pointcloud/hit_buffer", hit_buffer_, 5);
+  nh_->param<float>("/ogrid_pointcloud/uncertainty_as_hit", uncertainty_as_hit_, 0.95);
+  nh_->param<float>("/ogrid_pointcloud/not_hit_degrade", not_hit_degrade_, 0.01);
   // Sub's position relative to the ogrid
-  cv::Point2d where_sub = cv::Point2d(transform.getOrigin().x() / resolution + mat_ogrid.cols / 2,
-                                      transform.getOrigin().y() / resolution + mat_ogrid.rows / 2);
-
-  // List of intersections
-  std::vector<cv::Point> intersections;
-  intersections.push_back(where_sub);
+  cv::Point2d where_sub =
+      cv::Point2d(transform.getOrigin().x() / resolution + mat_ogrid.cols / 2 - mat_origin.x / resolution,
+                  transform.getOrigin().y() / resolution + mat_ogrid.rows / 2 - mat_origin.y / resolution);
 
   // Find first hits in an expanding circle
   for (float d_theta = 0.f; d_theta <= 2 * CV_PI; d_theta += 0.005)
   {
     cv::Point2d p_on_ray = get_first_hit(mat_ogrid, where_sub, d_theta, mat_ogrid.cols);
-    intersections.push_back(cv::Point(p_on_ray.x, p_on_ray.y));
   }
-
-  const cv::Point *pts = (const cv::Point *)cv::Mat(intersections).data;
-  int npts = cv::Mat(intersections).rows;
-  cv::fillPoly(mat_ogrid, &pts, &npts, 1, cv::Scalar((uchar)WAYPOINT_ERROR_TYPE::UNOCCUPIED));
 }