Merge pull request #1 from midemig/rgb_layer

feat:add RGB layer from point proyection in image

Author: fmrico

local_navigation/CMakeLists.txt

@@ -20,6 +20,8 @@ find_package(grid_map_msgs REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(pcl_ros REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(image_geometry REQUIRED)
 
 set(dependencies
   rclcpp
@@ -34,6 +36,8 @@ set(dependencies
   pcl_conversions
   pcl_ros
   nav_msgs
+  image_geometry
+  OpenCV
 )
 
 include_directories(

local_navigation/config/params.yaml

@@ -1,3 +1,12 @@
 gridmap_updater_node:
   ros__parameters:
     use_sim_time: true
+    camera_info_topic: /camera/color/camera_info
+    # camera_info_topic: /zed2/zed_node/rgb/camera_info
+    camera_topic: /camera/color/image_raw
+    # camera_topic: /camera/semseg/image
+    map_frame: map
+    robot_frame: base_link
+    # robot_frame: robot_base_link
+    camera_frame: camera_color_optical_frame
+    # camera_frame: zed2_left_camera_optical_frame

local_navigation/config/params_perro.yaml

@@ -0,0 +1,9 @@
+gridmap_updater_node:
+  ros__parameters:
+    use_sim_time: true
+    camera_info_topic: /camera/color/camera_info
+    camera_topic: /camera/color/image_raw
+    # camera_topic: /camera/semseg/image
+    map_frame: map
+    robot_frame: base_link
+    camera_frame: camera_color_optical_frame

local_navigation/config/params_summit.yaml

@@ -0,0 +1,9 @@
+gridmap_updater_node:
+  ros__parameters:
+    use_sim_time: true
+    camera_info_topic: /zed2/zed_node/rgb/camera_info
+    camera_topic: /zed2/zed_node/rgb/image_rect_color
+    lidar_topic: /robot/front_laser/points
+    map_frame: map
+    robot_frame: robot_base_link
+    camera_frame: zed2_left_camera_optical_frame

local_navigation/include/local_navigation/GridmapUpdaterNode.hpp

@@ -18,6 +18,8 @@
 #include <string>
 #include <memory>
 
+#include <tuple>
+
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/transform_broadcaster.h"
@@ -31,6 +33,11 @@
 
 #include "nav_msgs/msg/path.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "image_geometry/pinhole_camera_model.hpp"
+
+#include "opencv2/opencv.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -45,24 +52,41 @@ class GridmapUpdaterNode : public rclcpp::Node
   explicit GridmapUpdaterNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
+  void get_params();
   void init_gridmap();
   void reset_gridmap();
   void update_gridmap(
     const pcl::PointCloud<pcl::PointXYZ> & pc_map,
-    const pcl::PointCloud<pcl::PointXYZ> & pc_robot);
+    const pcl::PointCloud<pcl::PointXYZ> & pc_robot,
+    const pcl::PointCloud<pcl::PointXYZ> & pc_camera);
   void publish_gridmap(const builtin_interfaces::msg::Time & stamp);
 
   void init_colors();
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pc_sub_;
   rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr subscription_info_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_img_;
   rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr gridmap_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr img_pub_;
 
   void pc_callback(sensor_msgs::msg::PointCloud2::UniquePtr pc_in);
   void path_callback(nav_msgs::msg::Path::UniquePtr path_in);
+  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_;
+  std::string camera_info_topic_;
+  std::string camera_topic_;
+  std::string lidar_topic_;
+  std::string path_topic_;
+
+  bool camera_info_received_ {false};
 
-  std::string map_frame_id_ {"map"};
-  std::string robot_frame_id_ {"robot_base_link"};
   double resolution_gridmap_ {0.2};
   double size_x_ {100.0};
   double size_y_ {100.0};
@@ -76,10 +100,15 @@ class GridmapUpdaterNode : public rclcpp::Node
   std::shared_ptr<grid_map::GridMap> gridmap_;
   Eigen::MatrixXf em_;
   Eigen::MatrixXf tm_;
+  Eigen::MatrixXf cm_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
+  std::shared_ptr<image_geometry::PinholeCameraModel> camera_model_;
+
+  cv::Mat image_rgb_raw_;
+
   float color_unknown_;
   float color_free_;
   float color_obstacle_;

local_navigation/launch/local_navigation.launch.py

@@ -40,10 +40,7 @@ def generate_launch_description():
                                 # prefix=['xterm -e gdb -ex run  --args'],
                                 # prefix=['perf record --call-graph dwarf -o perf.data'],
                                 arguments=[],
-                                remappings=[
-                                  ('input_pc', '/robot/front_laser/points'),
-                                  ('input_path', '/path'),
-                                ])
+                                remappings=[])
 
     ld = LaunchDescription()
     ld.add_action(local_navigation_cmd)

local_navigation/launch/local_navigation_perro.launch.py

@@ -0,0 +1,58 @@
+# Copyright 2024 Intelligent Robotics Lab
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('local_navigation')
+    param_file = os.path.join(pkg_dir, 'config', 'params_perro.yaml')
+
+    lidarslam_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(
+            get_package_share_directory('lidarslam'),
+            'launch',
+            'lidarslam_perro.launch.py')))
+
+    statictf_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(
+            get_package_share_directory('go2_description'),
+            'launch',
+            'robot.launch.py')))
+    
+
+
+    local_navigation_cmd = Node(package='local_navigation',
+                                executable='local_navigation_program',
+                                output='screen',
+                                parameters=[param_file],
+                                # prefix=['xterm -e gdb -ex run  --args'],
+                                # prefix=['perf record --call-graph dwarf -o perf.data'],
+                                arguments=[],
+                                remappings=[])
+
+    ld = LaunchDescription()
+    ld.add_action(local_navigation_cmd)
+    ld.add_action(lidarslam_cmd)
+    ld.add_action(statictf_cmd)
+
+    return ld

local_navigation/launch/local_navigation_summit.launch.py

@@ -0,0 +1,49 @@
+# Copyright 2024 Intelligent Robotics Lab
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('local_navigation')
+    param_file = os.path.join(pkg_dir, 'config', 'params_summit.yaml')
+
+    lidarslam_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(
+            get_package_share_directory('lidarslam'),
+            'launch',
+            'lidarslam_summit.launch.py')))
+
+    local_navigation_cmd = Node(package='local_navigation',
+                                executable='local_navigation_program',
+                                output='screen',
+                                parameters=[param_file],
+                                # prefix=['xterm -e gdb -ex run  --args'],
+                                # prefix=['perf record --call-graph dwarf -o perf.data'],
+                                arguments=[],
+                                remappings=[])
+
+    ld = LaunchDescription()
+    ld.add_action(local_navigation_cmd)
+    ld.add_action(lidarslam_cmd)
+
+    return ld

local_navigation/package.xml

@@ -20,6 +20,7 @@
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>nav_msgs</depend>
+  <depend>image_geometry</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>