Lintering and style

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Author: fmrico

local_navigation/include/local_navigation/GridmapUpdaterNode.hpp

@@ -75,8 +75,6 @@ class GridmapUpdaterNode : public rclcpp::Node
   void topic_callback_info(sensor_msgs::msg::CameraInfo::UniquePtr msg);
   void image_callback(sensor_msgs::msg::Image::UniquePtr msg);
 
-  std::tuple<float, int, int> get_point_color(pcl::PointXYZ point);
-
   std::string map_frame_id_;
   std::string robot_frame_id_;
   std::string camera_frame_id_;
@@ -85,8 +83,6 @@ class GridmapUpdaterNode : public rclcpp::Node
   std::string lidar_topic_;
   std::string path_topic_;
 
-  bool camera_info_received_ {false};
-
   double resolution_gridmap_ {0.2};
   double size_x_ {100.0};
   double size_y_ {100.0};

local_navigation/launch/local_navigation_perro.launch.py

@@ -38,8 +38,6 @@ def generate_launch_description():
             get_package_share_directory('go2_description'),
             'launch',
             'robot.launch.py')))
-    
-
 
     local_navigation_cmd = Node(package='local_navigation',
                                 executable='local_navigation_program',

local_navigation/src/local_navigation/GridmapUpdaterNode.cpp

@@ -45,7 +45,6 @@ GridmapUpdaterNode::GridmapUpdaterNode(const rclcpp::NodeOptions & options)
   tf_buffer_(this->get_clock()),
   tf_listener_(tf_buffer_)
 {
-
   get_params();
 
   init_colors();
@@ -82,7 +81,7 @@ GridmapUpdaterNode::get_params()
   path_topic_ = this->get_parameter("path_topic").as_string();
   map_frame_id_ = this->get_parameter("map_frame").as_string();
   robot_frame_id_ = this->get_parameter("robot_frame").as_string();
-  camera_frame_id_  = this->get_parameter("camera_frame").as_string();
+  camera_frame_id_ = this->get_parameter("camera_frame").as_string();
 }
 
 void
@@ -97,7 +96,7 @@ GridmapUpdaterNode::init_gridmap()
 
   // Adding layers
   gridmap_->add("elevation");
-  gridmap_->add("RGB"); 
+  gridmap_->add("RGB");
   gridmap_->add("transversality");
 
   reset_gridmap();
@@ -125,32 +124,68 @@ GridmapUpdaterNode::reset_gridmap()
 
   // Set init values in matrixes
   for (auto i = 0; i < gridmap_->getSize()(0); i++) {
-    for (auto j = 0; j < gridmap_->getSize()(1); j++) {
-      em_(i, j) = NAN;
-      tm_(i, j) = color_unknown_;
-      cm_(i, j) = 0;
-    }
+      for (auto j = 0; j < gridmap_->getSize()(1); j++) {
+        em_(i, j) = NAN;
+        tm_(i, j) = color_unknown_;
+        cm_(i, j) = 0;
+      }
   }
 
   // Dump matrixes to gridmap layers
   for (auto i = 0; i < gridmap_->getSize()(0); i++) {
-    for (auto j = 0; j < gridmap_->getSize()(1); j++) {
-      grid_map::Index map_index(i, j);
-      gridmap_->at("elevation", map_index) = em_(i, j);
-      gridmap_->at("transversality", map_index) = tm_(i, j);
-      gridmap_->at("RGB", map_index) = cm_(i, j);
+      for (auto j = 0; j < gridmap_->getSize()(1); j++) {
+        grid_map::Index map_index(i, j);
+        gridmap_->at("elevation", map_index) = em_(i, j);
+        gridmap_->at("transversality", map_index) = tm_(i, j);
+        gridmap_->at("RGB", map_index) = cm_(i, j);
+      }
+  }
+}
+
+std::tuple<float, int, int>
+get_point_color(
+  const pcl::PointXYZ & point, const image_geometry::PinholeCameraModel & camera_model,
+  const cv::Mat & image_rgb_raw)
+{
+  cv::Mat world_point_fromCamera = (cv::Mat_<double>(3, 1) << point.x, point.y, point.z);
+  cv::Point2d point_2d = camera_model.project3dToPixel(cv::Point3d(point.x, point.y, point.z));
+
+  double point_x = point_2d.x;
+  double point_y = point_2d.y;
+
+  if (point_x > 0 && point_x < image_rgb_raw.cols && point_y > 0 && point_y < image_rgb_raw.rows) {
+    if (image_rgb_raw.type() == CV_8UC3) {
+      cv::Vec3b color = image_rgb_raw.at<cv::Vec3b>(static_cast<int>(point_y),
+        static_cast<int>(point_x));
+      Eigen::Vector3i color_eigen(color[0], color[1], color[2]);
+      float color_value;
+      grid_map::colorVectorToValue(color_eigen, color_value);
+      return {color_value, point_x, point_y};
+    } else if (image_rgb_raw.type() == CV_8UC1) {
+      float color_value = image_rgb_raw.at<uint8_t>(static_cast<int>(point_y),
+        static_cast<int>(point_x));
+      return {color_value, point_x, point_y};
+    } else if (image_rgb_raw.type() == CV_8UC4) {
+      cv::Vec4b color = image_rgb_raw.at<cv::Vec4b>(static_cast<int>(point_y),
+        static_cast<int>(point_x));
+      Eigen::Vector3i color_eigen(color[2], color[1], color[0]);
+      float color_value;
+      grid_map::colorVectorToValue(color_eigen, color_value);
+      return {color_value, point_x, point_y};
+    } else {
+      return {-1.0, -1.0, -1.0};
     }
+  } else {
+    return {-1.0, -1.0, -1.0};
   }
 }
 
 void
 GridmapUpdaterNode::update_gridmap(
   const pcl::PointCloud<pcl::PointXYZ> & pc_map,
   const pcl::PointCloud<pcl::PointXYZ> & pc_robot,
-  const pcl::PointCloud<pcl::PointXYZ> & pc_camera
-  )
+  const pcl::PointCloud<pcl::PointXYZ> & pc_camera)
 {
-
   // RCLCPP_INFO(get_logger(), "Updating gridmap");
   for (size_t i = 0; i < pc_map.size(); i++) {
     const auto & point = pc_map[i];
@@ -203,30 +238,18 @@ GridmapUpdaterNode::update_gridmap(
 
     gridmap_->at("elevation", idx) = em_(idx(0), idx(1));
 
-
-
-    if (point_camera.z > 0) // Prevent to proyect points behind the camera 
+    if (camera_model_ != nullptr && !image_rgb_raw_.empty() &&
+      point_camera.z > 0)  // Prevent to proyect points behind the camera
     {
-      auto [color, p_x, p_y] = get_point_color(point_camera);
-      if (color > 0)
-      {
+      auto [color, p_x, p_y] = get_point_color(point_camera, *camera_model_, image_rgb_raw_);
+      if (color > 0) {
         cm_(idx(0), idx(1)) = color;
         gridmap_->at("RGB", idx) = color;
-
-        // // Debug
-        // image_rgb_raw_.at<cv::Vec3b>(p_y, p_x) = color;
       }
-    }
-    else
-    {
+    } else {
       gridmap_->at("RGB", idx) = cm_(idx(0), idx(1));
     }
   }
-  // // DEBUG Proyected points
-  // cv_bridge::CvImage cv_image;
-  // cv_image.image = image_rgb_raw_;
-  // cv_image.encoding = sensor_msgs::image_encodings::RGB8;
-  // img_pub_->publish(*cv_image.toImageMsg().get());
 }
 
 void
@@ -269,8 +292,6 @@ transform_cloud(
 void
 GridmapUpdaterNode::pc_callback(sensor_msgs::msg::PointCloud2::UniquePtr pc_in)
 {
-  // RCLCPP_INFO(get_logger(), "PointCloud received");
-
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::fromROSMsg(*pc_in, *pcl_cloud);
 
@@ -326,103 +347,34 @@ GridmapUpdaterNode::path_callback(nav_msgs::msg::Path::UniquePtr path_in)
 void
 GridmapUpdaterNode::topic_callback_info(sensor_msgs::msg::CameraInfo::UniquePtr msg)
 {
-  // RCLCPP_INFO(get_logger(), "Camera info received");
-
   camera_model_ = std::make_shared<image_geometry::PinholeCameraModel>();
   camera_model_->fromCameraInfo(*msg);
 
-  camera_info_received_ = true;
-
   subscription_info_ = nullptr;
 }
 
 void
 GridmapUpdaterNode::image_callback(sensor_msgs::msg::Image::UniquePtr msg)
 {
-  // RCLCPP_INFO(get_logger(), "Img received");
-
   cv_bridge::CvImagePtr image_rgb_ptr;
 
-  try
-  {
-    if (msg->encoding == sensor_msgs::image_encodings::RGB8)
-    {
+  try {
+    if (msg->encoding == sensor_msgs::image_encodings::RGB8) {
       image_rgb_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8);
-    }
-    else if (msg->encoding == sensor_msgs::image_encodings::MONO8)
-    {
+    } else if (msg->encoding == sensor_msgs::image_encodings::MONO8) {
       image_rgb_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::MONO8);
-    }
-    else if (msg->encoding == sensor_msgs::image_encodings::BGRA8)
-    {
+    } else if (msg->encoding == sensor_msgs::image_encodings::BGRA8) {
       image_rgb_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::BGRA8);
-    }
-    else
-    {
+    } else {
       RCLCPP_ERROR(get_logger(), "Unsupported encoding %s", msg->encoding.c_str());
       return;
     }
-  }
-  catch (cv_bridge::Exception & e) 
-  {
+  } catch (cv_bridge::Exception & e) {
     RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what());
     return;
   }
-  image_rgb_raw_ = image_rgb_ptr->image;
-}
-
-std::tuple<float, int, int>
-GridmapUpdaterNode::get_point_color(pcl::PointXYZ point)
-{
-
-  if(!camera_info_received_)
-  {
-    RCLCPP_ERROR(get_logger(), "Error: Camera info not received");
-    return {-1.0, -1.0, -1.0};
-  }
-
-  cv::Mat world_point_fromCamera = (cv::Mat_<double>(3, 1) << point.x, point.y, point.z);
-
-  cv::Mat intrinsic_matrix = cv::Mat(camera_model_->intrinsicMatrix());
 
-  cv::Point2d point_2d = camera_model_->project3dToPixel(cv::Point3d(point.x, point.y, point.z));
-
-  double point_x = point_2d.x;
-  double point_y = point_2d.y;
-
-  if (point_x > 0 && point_x < image_rgb_raw_.cols && point_y > 0 && point_y < image_rgb_raw_.rows) 
-  {
-    if (image_rgb_raw_.type() == CV_8UC3)
-    {
-      cv::Vec3b color = image_rgb_raw_.at<cv::Vec3b>(int(point_y), int(point_x));
-      Eigen::Vector3i color_eigen(color[0], color[1], color[2]);
-      float color_value;
-      grid_map::colorVectorToValue(color_eigen, color_value);
-      return {color_value, point_x, point_y};
-    }
-    else if (image_rgb_raw_.type() == CV_8UC1)
-    {
-      float color_value = image_rgb_raw_.at<uint8_t>(int(point_y), int(point_x));
-      return {color_value, point_x, point_y};
-    }
-    else if (image_rgb_raw_.type() == CV_8UC4)
-    {
-      cv::Vec4b color = image_rgb_raw_.at<cv::Vec4b>(int(point_y), int(point_x));
-      Eigen::Vector3i color_eigen(color[2], color[1], color[0]);
-      float color_value;
-      grid_map::colorVectorToValue(color_eigen, color_value);
-      return {color_value, point_x, point_y};
-    }
-    else
-    {
-      RCLCPP_ERROR(get_logger(), "Error: Image encoding not supported");
-      return {-1.0, -1.0, -1.0};
-    }
-  } 
-  else 
-  {
-    return {-1.0, -1.0, -1.0};
-  }
+  image_rgb_raw_ = image_rgb_ptr->image;
 }
 
 }  // namespace local_navigation