Fix image and get patches

Signed-off-by: Francisco Martín Rico <[email protected]>

local_navigation/CMakeLists.txt

@@ -59,13 +59,16 @@ add_executable(local_navigation_program
 ament_target_dependencies(local_navigation_program ${dependencies})
 target_link_libraries(local_navigation_program ${PROJECT_NAME})
 
+add_executable(summit_correct_zed_tf src/tools/summit_correct_zed_tf.cpp)
+ament_target_dependencies(summit_correct_zed_tf ${dependencies})
+
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-install(TARGETS local_navigation_program
+install(TARGETS local_navigation_program summit_correct_zed_tf
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 

local_navigation/README.md

@@ -7,7 +7,9 @@ This package contains our work about traversity
 1. In terminal 1 play a rosbag:
 
 ```
-ros2 bag play rosbags/trial_0 --clock -p
+ros2 bag play rosbags/summit/test_1_1 --clock -p --remap /tf_static:=/rosbag/tf_static /tf:=/rosbag/tf 
+ros2 run local_navigation summit_correct_zed_tf --ros-args -p use_sim_time:=True
+
 ```
 
 2. In terminal 2 launch the nodes:

local_navigation/config/params_go2.yaml

@@ -0,0 +1,57 @@
+gridmap_updater_node:
+  ros__parameters:
+    use_sim_time: True
+    camera_info_topic: /camera/color/camera_info
+    camera_topic: /camera/color/image_raw
+    lidar_topic: /pointcloud
+    map_frame: map
+    robot_frame: base_link
+    camera_frame: camera_color_optical_frame
+
+scan_matcher:
+  ros__parameters:
+    use_sim_time: True
+    global_frame_id: "map"
+
+    robot_frame_id: "base_link"
+    odom_frame_id: "odom"
+    registration_method: "NDT"
+    ndt_resolution: 2.0
+    ndt_num_threads: 2
+    gicp_corr_dist_threshold: 5.0
+    trans_for_mapupdate: 1.5
+    vg_size_for_input: 0.5
+    vg_size_for_map: 0.2
+    use_min_max_filter: true
+    scan_min_range: 1.0
+    scan_max_range: 200.0
+    scan_period: 0.2
+    map_publish_period: 15.0
+    num_targeted_cloud: 20
+    set_initial_pose: true
+    initial_pose_x: 0.0
+    initial_pose_y: 0.0
+    initial_pose_z: 0.0
+    initial_pose_qx: 0.0
+    initial_pose_qy: 0.0
+    initial_pose_qz: 0.0
+    initial_pose_qw: 1.0
+    use_imu: false
+    use_odom: false
+    debug_flag: false
+
+graph_based_slam:
+    ros__parameters:
+      use_sim_time: True
+      registration_method: "NDT"
+      ndt_resolution: 1.0
+      ndt_num_threads: 2
+      voxel_leaf_size: 0.2
+      loop_detection_period: 3000
+      threshold_loop_closure_score: 0.7
+      distance_loop_closure: 100.0
+      range_of_searching_loop_closure: 20.0
+      search_submap_num: 2
+      num_adjacent_pose_cnstraints: 5
+      use_save_map_in_loop: true
+      debug_flag: true

local_navigation/config/params_summit.yaml

@@ -7,3 +7,53 @@ gridmap_updater_node:
     map_frame: map
     robot_frame: robot_base_link
     camera_frame: zed2_left_camera_optical_frame
+    size_x: 200.0
+    size_y: 200.0
+
+scan_matcher:
+  ros__parameters:
+    use_sim_time: True
+    global_frame_id: "map"
+
+    robot_frame_id: "robot_base_link"
+    odom_frame_id: "robot_odom"
+    registration_method: "NDT"
+    ndt_resolution: 2.0
+    ndt_num_threads: 2
+    gicp_corr_dist_threshold: 5.0
+    trans_for_mapupdate: 1.5
+    vg_size_for_input: 0.5
+    vg_size_for_map: 0.2
+    use_min_max_filter: true
+    scan_min_range: 1.0
+    scan_max_range: 200.0
+    scan_period: 0.2
+    map_publish_period: 15.0
+    num_targeted_cloud: 20
+    set_initial_pose: true
+    initial_pose_x: 0.0
+    initial_pose_y: 0.0
+    initial_pose_z: 0.0
+    initial_pose_qx: 0.0
+    initial_pose_qy: 0.0
+    initial_pose_qz: 0.0
+    initial_pose_qw: 1.0
+    use_imu: false
+    use_odom: false
+    debug_flag: false
+
+graph_based_slam:
+    ros__parameters:
+      use_sim_time: True
+      registration_method: "NDT"
+      ndt_resolution: 1.0
+      ndt_num_threads: 2
+      voxel_leaf_size: 0.2
+      loop_detection_period: 3000
+      threshold_loop_closure_score: 0.7
+      distance_loop_closure: 100.0
+      range_of_searching_loop_closure: 20.0
+      search_submap_num: 2
+      num_adjacent_pose_cnstraints: 5
+      use_save_map_in_loop: true
+      debug_flag: true

local_navigation/include/local_navigation/GridmapUpdaterNode.hpp

@@ -55,43 +55,64 @@ class GridmapUpdaterNode : public rclcpp::Node
   void get_params();
   void init_gridmap();
   void reset_gridmap();
-  void update_gridmap(
+  void update_gridmap_elevation(
     const pcl::PointCloud<pcl::PointXYZ> & pc_map,
     const pcl::PointCloud<pcl::PointXYZ> & pc_robot,
     const pcl::PointCloud<pcl::PointXYZ> & pc_camera);
+  void update_gridmap_texture(
+    const cv::Mat & image,
+    const image_geometry::PinholeCameraModel & camera_model,
+    builtin_interfaces::msg::Time stamp);
   void publish_gridmap(const builtin_interfaces::msg::Time & stamp);
-
   void init_colors();
 
+  std::tuple<tf2::Stamped<tf2::Transform>, bool, std::string>
+  get_frame_tf(const std::string & frame, const builtin_interfaces::msg::Time & stamp);
+
+  std::tuple<float, bool> get_color_in_gridmap(
+    const cv::Mat & image,
+    const image_geometry::PinholeCameraModel & camera_model,
+    const tf2::Stamped<tf2::Transform> & map2camera,
+    const tf2::Vector3 & gpoint);
+
+  void control_cycle();
+
+  std::tuple<grid_map::GridMap, bool>
+  create_patch_in_pos(const tf2::Stamped<tf2::Transform> & pos);
+  void publish_patch(const grid_map::GridMap & patch);
+
   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_;
+  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr patch_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::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_;
+  std::string map_frame_id_ {"map"};
+  std::string robot_frame_id_ {"base_link"};
+  std::string camera_frame_id_ {"camera_color_optical_frame"};
+  std::string camera_info_topic_ {"/camera/color/camera_info"};
+  std::string camera_topic_ {"/camera/color/image_raw"};
+  std::string lidar_topic_ {"/lidar_points"};
+  std::string path_topic_ {"/path"};
 
   double resolution_gridmap_ {0.2};
-  double size_x_ {100.0};
-  double size_y_ {100.0};
+  double size_x_ {200.0};
+  double size_y_ {200.0};
   double infl_radious_x_ {10.0};
   double infl_radious_y_ {10.0};
   double robot_radious_min_x_ {-4.0};
   double robot_radious_max_x_ {1.0};
   double robot_radious_y_ {1.0};
   double robot_radious_ {0.5};
+  double patch_size_ {1.0};
+  float patch_distance_ {1.0};
 
   std::shared_ptr<grid_map::GridMap> gridmap_;
   Eigen::MatrixXf em_;
@@ -103,11 +124,13 @@ class GridmapUpdaterNode : public rclcpp::Node
 
   std::shared_ptr<image_geometry::PinholeCameraModel> camera_model_;
 
-  cv::Mat image_rgb_raw_;
-
   float color_unknown_;
   float color_free_;
   float color_obstacle_;
+
+  rclcpp::TimerBase::SharedPtr cycle_timer_;
+  tf2::Stamped<tf2::Transform> last_map2robot_;
+  bool started_odom_ {false};
 };
 
 }  // namespace local_navigation

local_navigation/launch/local_navigation.launch.py

@@ -31,7 +31,13 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(os.path.join(
             get_package_share_directory('lidarslam'),
             'launch',
-            'lidarslam.launch.py')))
+            'lidarslam.launch.py')),
+        launch_arguments={
+            'main_param_dir': os.path.join(pkg_dir,
+                                           'config',
+                                           'lidarslam_summit.yaml')
+        }
+    )
 
     local_navigation_cmd = Node(package='local_navigation',
                                 executable='local_navigation_program',

local_navigation/launch/local_navigation_perro.launch.py → local_navigation/launch/local_navigation_go2.launch.py

@@ -17,27 +17,13 @@
 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')))
+    param_file = os.path.join(pkg_dir, 'config', 'params_go2.yaml')
 
     local_navigation_cmd = Node(package='local_navigation',
                                 executable='local_navigation_program',
@@ -48,9 +34,27 @@ def generate_launch_description():
                                 arguments=[],
                                 remappings=[])
 
+    mapping = Node(
+        package='scanmatcher',
+        executable='scanmatcher_node',
+        parameters=[param_file],
+        remappings=[
+            ('/input_cloud', '/lidar_points'),
+            # ('/imu','/imu'),
+            ],
+        output='log'
+        )
+
+    graphbasedslam = Node(
+        package='graph_based_slam',
+        executable='graph_based_slam_node',
+        parameters=[param_file],
+        output='log'
+        )
+
     ld = LaunchDescription()
     ld.add_action(local_navigation_cmd)
-    ld.add_action(lidarslam_cmd)
-    ld.add_action(statictf_cmd)
+    ld.add_action(mapping)
+    ld.add_action(graphbasedslam)
 
     return ld

local_navigation/launch/local_navigation_summit.launch.py

@@ -17,8 +17,8 @@
 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.actions import IncludeLaunchDescription
+# from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 from launch_ros.actions import Node
 
@@ -27,12 +27,6 @@ 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',
@@ -42,8 +36,27 @@ def generate_launch_description():
                                 arguments=[],
                                 remappings=[])
 
+    mapping = Node(
+        package='scanmatcher',
+        executable='scanmatcher_node',
+        parameters=[param_file],
+        remappings=[
+            ('/input_cloud', '/robot/front_laser/points'),
+            # ('/imu','/robot/imu/data'),
+            ],
+        output='screen'
+        )
+
+    graphbasedslam = Node(
+        package='graph_based_slam',
+        executable='graph_based_slam_node',
+        parameters=[param_file],
+        output='screen'
+        )
+
     ld = LaunchDescription()
     ld.add_action(local_navigation_cmd)
-    ld.add_action(lidarslam_cmd)
+    ld.add_action(mapping)
+    ld.add_action(graphbasedslam)
 
     return ld