OGRID: ignore points below dvl, add waypoint visuals

Signed-off-by: RustyBamboo <dvolya@gmail.com>

legacy/c3_trajectory_generator/include/waypoint_validity.hpp

@@ -24,6 +24,10 @@ class WaypointValidity
   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 +37,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 = 120);
+
   // 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/AttitudeHelpers.cpp

@@ -17,6 +17,5 @@ Matrix3d AttitudeHelpers::EulerToRotation(const Vector3d& rpy)
 
   return (Matrix3d() << cpsi * ctheta, -spsi * cphi + cpsi * stheta * sphi, spsi * sphi + cphi * cphi * stheta,
           spsi * ctheta, cpsi * cphi + sphi * stheta * spsi, -cpsi * sphi + stheta * spsi * cphi, -stheta,
-          ctheta * sphi, ctheta * cphi)
-      .finished();
+          ctheta * sphi, ctheta * cphi).finished();
 }

legacy/c3_trajectory_generator/src/node.cpp

@@ -60,9 +60,9 @@ PoseTwist PoseTwist_from_PointWithAcceleration(const subjugator::C3Trajectory::P
   res.pose.position = vec2xyz<Point>(p.q.head(3));
   quaternionTFToMsg(orient, res.pose.orientation);
 
-  Eigen::Matrix3d worldangvel_from_eulerrates = (Eigen::Matrix3d() << 1, 0, -sin(p.q[4]), 0, cos(p.q[3]),
-                                                 sin(p.q[3]) * cos(p.q[4]), 0, -sin(p.q[3]), cos(p.q[3]) * cos(p.q[4]))
-                                                    .finished();
+  Eigen::Matrix3d worldangvel_from_eulerrates =
+      (Eigen::Matrix3d() << 1, 0, -sin(p.q[4]), 0, cos(p.q[3]), sin(p.q[3]) * cos(p.q[4]), 0, -sin(p.q[3]),
+       cos(p.q[3]) * cos(p.q[4])).finished();
 
   res.twist.linear = vec2xyz<Vector3>(tf::Matrix3x3(orient.inverse()) * vec2vec(p.qdot.head(3)));
   res.twist.angular = vec2xyz<Vector3>(worldangvel_from_eulerrates * p.qdot.tail(3));
@@ -136,7 +136,10 @@ struct Node
     , waypoint_validity_(nh)
   {
     // Callback to reset trajectory when (un)killing
-    auto reset_traj = [this](ros_alarms::AlarmProxy a) { this->c3trajectory.reset(); };
+    auto reset_traj = [this](ros_alarms::AlarmProxy a)
+    {
+      this->c3trajectory.reset();
+    };
     kill_listener.addRaiseCb(reset_traj);
 
     // Make sure alarm integration is ok
@@ -209,13 +212,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));
+
       // 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), 130);
         if (checkWPResult.second ==
             WAYPOINT_ERROR_TYPE::UNKNOWN)  // if unknown, check if there's a huge displacement with the new waypoint
         {
@@ -296,6 +302,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

@@ -49,8 +49,9 @@ std::pair<bool, WAYPOINT_ERROR_TYPE> WaypointValidity::is_waypoint_valid(const g
                                   waypoint.position.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 +71,32 @@ 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)
+{
+  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 == 200)
+    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/OGridGen.hpp

@@ -27,29 +27,34 @@
 
 #include <Classification.hpp>
 
+#include <mil_msgs/RangeStamped.h>
+
 class OGridGen
 {
 public:
   OGridGen();
   void publish_ogrid(const ros::TimerEvent &);
 
   void callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg);
+  void dvl_callback(const mil_msgs::RangeStampedConstPtr &dvl);
 
 private:
   ros::NodeHandle nh_;
   ros::Subscriber sub_to_imaging_sonar_;
+  ros::Subscriber sub_to_dvl_;
 
   tf::TransformListener listener_;
 
   ros::Publisher pub_grid_;
   ros::Publisher pub_point_cloud_filtered_;
   ros::Publisher pub_point_cloud_raw_;
+  ros::Publisher pub_point_cloud_plane_;
   ros::Timer timer_;
 
   cv::Mat mat_ogrid_;
   float ogrid_size_;
   float resolution_;
-  float pool_depth_;
+  double dvl_range_;
   int min_intensity_;
 
   ros::ServiceClient service_get_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/OGridGen.cpp

@@ -8,13 +8,14 @@ OGridGen::OGridGen() : nh_(ros::this_node::getName()), classification_(&nh_)
   pub_grid_ = nh_.advertise<nav_msgs::OccupancyGrid>("ogrid", 10, true);
   pub_point_cloud_filtered_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZI>>("point_cloud/filtered", 1);
   pub_point_cloud_raw_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZI>>("point_cloud/raw", 1);
+  pub_point_cloud_plane_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZI>>("point_cloud/plane", 1);
 
   // Resolution is meters/pixel
   nh_.param<float>("resolution", resolution_, 0.2f);
   nh_.param<float>("ogrid_size", ogrid_size_, 91.44);
   // Ignore points that are below the potential pool
-  nh_.param<float>("pool_depth", pool_depth_, 7.f);
   nh_.param<int>("min_intensity", min_intensity_, 2000);
+  dvl_range_ = 0;
 
   // Buffer that will only hold a certain amount of points
   int point_cloud_buffer_Size;
@@ -27,9 +28,15 @@ OGridGen::OGridGen() : nh_(ros::this_node::getName()), classification_(&nh_)
   // Run the publisher
   timer_ = nh_.createTimer(ros::Duration(0.3), std::bind(&OGridGen::publish_ogrid, this, std::placeholders::_1));
   sub_to_imaging_sonar_ = nh_.subscribe("/blueview_driver/ranges", 1, &OGridGen::callback, this);
+  sub_to_dvl_ = nh_.subscribe("/dvl/ranges", 1, &OGridGen::dvl_callback, this);
 
   mat_ogrid_ = cv::Mat::zeros(int(ogrid_size_ / resolution_), int(ogrid_size_ / resolution_), CV_8U);
 }
+
+void OGridGen::dvl_callback(const mil_msgs::RangeStampedConstPtr &dvl)
+{
+  dvl_range_ = dvl->range;
+}
 /*
   Looped based on timer_.
   Reads point_cloud_buffer_ and publishes a PointCloud2
@@ -131,6 +138,7 @@ void OGridGen::callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg)
     ROS_DEBUG_STREAM("Did not get TF for imaging sonar");
     return;
   }
+  pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_plane(new pcl::PointCloud<pcl::PointXYZI>());
   for (size_t i = 0; i < ping_msg->ranges.size(); ++i)
   {
     if (ping_msg->intensities.at(i) > min_intensity_)
@@ -149,10 +157,16 @@ void OGridGen::callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg)
       point.x = newVec.x() + transform_.getOrigin().x();
       point.y = newVec.y() + transform_.getOrigin().y();
       point.z = newVec.z() + transform_.getOrigin().z();
+      if (point.z + dvl_range_ > 0)
+        continue;
       point.intensity = ping_msg->intensities.at(i);
       point_cloud_buffer_.push_back(point);
+      point_cloud_plane->push_back(point);
     }
   }
+  point_cloud_plane->header.frame_id = "map";
+  pcl_conversions::toPCL(ros::Time::now(), point_cloud_plane->header.stamp);
+  pub_point_cloud_plane_.publish(point_cloud_plane);
 }
 
 int main(int argc, char **argv)