From f438c1777e611e8d7fdeddb0aaf0e2f80a59da27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 12 Aug 2024 13:54:24 +0200 Subject: [PATCH 1/2] Fix image and get patches MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- local_navigation/CMakeLists.txt | 5 +- local_navigation/README.md | 4 +- local_navigation/config/params_go2.yaml | 57 ++++ local_navigation/config/params_perro.yaml | 9 - local_navigation/config/params_summit.yaml | 50 ++++ .../local_navigation/GridmapUpdaterNode.hpp | 49 +++- .../install/.colcon_install_layout | 1 - .../launch/local_navigation.launch.py | 8 +- ...unch.py => local_navigation_go2.launch.py} | 38 +-- .../launch/local_navigation_summit.launch.py | 31 ++- .../local_navigation/GridmapUpdaterNode.cpp | 262 +++++++++++++++--- .../src/tools/summit_correct_zed_tf.cpp | 107 +++++++ 12 files changed, 536 insertions(+), 85 deletions(-) create mode 100644 local_navigation/config/params_go2.yaml delete mode 100644 local_navigation/config/params_perro.yaml delete mode 100644 local_navigation/install/.colcon_install_layout rename local_navigation/launch/{local_navigation_perro.launch.py => local_navigation_go2.launch.py} (67%) create mode 100644 local_navigation/src/tools/summit_correct_zed_tf.cpp diff --git a/local_navigation/CMakeLists.txt b/local_navigation/CMakeLists.txt index 581a7ed..fd20e02 100644 --- a/local_navigation/CMakeLists.txt +++ b/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} ) diff --git a/local_navigation/README.md b/local_navigation/README.md index 03da2fd..7334d93 100644 --- a/local_navigation/README.md +++ b/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: diff --git a/local_navigation/config/params_go2.yaml b/local_navigation/config/params_go2.yaml new file mode 100644 index 0000000..293fa4e --- /dev/null +++ b/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 diff --git a/local_navigation/config/params_perro.yaml b/local_navigation/config/params_perro.yaml deleted file mode 100644 index 0ca195e..0000000 --- a/local_navigation/config/params_perro.yaml +++ /dev/null @@ -1,9 +0,0 @@ -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 diff --git a/local_navigation/config/params_summit.yaml b/local_navigation/config/params_summit.yaml index 6968484..9868686 100644 --- a/local_navigation/config/params_summit.yaml +++ b/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 diff --git a/local_navigation/include/local_navigation/GridmapUpdaterNode.hpp b/local_navigation/include/local_navigation/GridmapUpdaterNode.hpp index 4d373db..1da80d8 100644 --- a/local_navigation/include/local_navigation/GridmapUpdaterNode.hpp +++ b/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 & pc_map, const pcl::PointCloud & pc_robot, const pcl::PointCloud & 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, bool, std::string> + get_frame_tf(const std::string & frame, const builtin_interfaces::msg::Time & stamp); + + std::tuple get_color_in_gridmap( + const cv::Mat & image, + const image_geometry::PinholeCameraModel & camera_model, + const tf2::Stamped & map2camera, + const tf2::Vector3 & gpoint); + + void control_cycle(); + + std::tuple + create_patch_in_pos(const tf2::Stamped & pos); + void publish_patch(const grid_map::GridMap & patch); + rclcpp::Subscription::SharedPtr pc_sub_; rclcpp::Subscription::SharedPtr path_sub_; rclcpp::Subscription::SharedPtr subscription_info_; rclcpp::Subscription::SharedPtr subscription_img_; rclcpp::Publisher::SharedPtr gridmap_pub_; rclcpp::Publisher::SharedPtr img_pub_; + rclcpp::Publisher::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 gridmap_; Eigen::MatrixXf em_; @@ -103,11 +124,13 @@ class GridmapUpdaterNode : public rclcpp::Node std::shared_ptr camera_model_; - cv::Mat image_rgb_raw_; - float color_unknown_; float color_free_; float color_obstacle_; + + rclcpp::TimerBase::SharedPtr cycle_timer_; + tf2::Stamped last_map2robot_; + bool started_odom_ {false}; }; } // namespace local_navigation diff --git a/local_navigation/install/.colcon_install_layout b/local_navigation/install/.colcon_install_layout deleted file mode 100644 index 3aad533..0000000 --- a/local_navigation/install/.colcon_install_layout +++ /dev/null @@ -1 +0,0 @@ -isolated diff --git a/local_navigation/launch/local_navigation.launch.py b/local_navigation/launch/local_navigation.launch.py index 8b60e37..34b11e6 100755 --- a/local_navigation/launch/local_navigation.launch.py +++ b/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', diff --git a/local_navigation/launch/local_navigation_perro.launch.py b/local_navigation/launch/local_navigation_go2.launch.py similarity index 67% rename from local_navigation/launch/local_navigation_perro.launch.py rename to local_navigation/launch/local_navigation_go2.launch.py index 6d03ffc..066e077 100755 --- a/local_navigation/launch/local_navigation_perro.launch.py +++ b/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 diff --git a/local_navigation/launch/local_navigation_summit.launch.py b/local_navigation/launch/local_navigation_summit.launch.py index 69a91ab..23f6478 100755 --- a/local_navigation/launch/local_navigation_summit.launch.py +++ b/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 diff --git a/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp b/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp index 70a78d7..a681216 100644 --- a/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp +++ b/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp @@ -42,7 +42,7 @@ using namespace std::chrono_literals; GridmapUpdaterNode::GridmapUpdaterNode(const rclcpp::NodeOptions & options) : Node("gridmap_updater_node", options), - tf_buffer_(this->get_clock()), + tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { get_params(); @@ -51,7 +51,7 @@ GridmapUpdaterNode::GridmapUpdaterNode(const rclcpp::NodeOptions & options) init_gridmap(); pc_sub_ = create_subscription( - lidar_topic_, 10, std::bind(&GridmapUpdaterNode::pc_callback, this, _1)); + lidar_topic_, 100, std::bind(&GridmapUpdaterNode::pc_callback, this, _1)); path_sub_ = create_subscription( path_topic_, 10, std::bind(&GridmapUpdaterNode::path_callback, this, _1)); subscription_info_ = create_subscription( @@ -61,27 +61,38 @@ GridmapUpdaterNode::GridmapUpdaterNode(const rclcpp::NodeOptions & options) camera_topic_, 1, std::bind(&GridmapUpdaterNode::image_callback, this, _1)); gridmap_pub_ = create_publisher("grid_map", 10); + patch_pub_ = create_publisher("patch", 10); img_pub_ = create_publisher("img_proy_debug", 10); + + cycle_timer_ = create_wall_timer(1s, std::bind(&GridmapUpdaterNode::control_cycle, this)); } void GridmapUpdaterNode::get_params() { - this->declare_parameter("camera_info_topic", "/camera/color/camera_info"); - this->declare_parameter("camera_topic", "/camera/color/image_raw"); - this->declare_parameter("lidar_topic", "/lidar_points"); - this->declare_parameter("path_topic", "/path"); - this->declare_parameter("map_frame", "map"); - this->declare_parameter("robot_frame", "base_link"); - this->declare_parameter("camera_frame", "camera_color_optical_frame"); - - camera_info_topic_ = this->get_parameter("camera_info_topic").as_string(); - camera_topic_ = this->get_parameter("camera_topic").as_string(); - lidar_topic_ = this->get_parameter("lidar_topic").as_string(); - 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(); + declare_parameter("camera_info_topic", camera_info_topic_); + declare_parameter("camera_topic", camera_topic_); + declare_parameter("lidar_topic", lidar_topic_); + declare_parameter("path_topic", path_topic_); + declare_parameter("map_frame", map_frame_id_); + declare_parameter("robot_frame", robot_frame_id_); + declare_parameter("camera_frame", camera_frame_id_); + declare_parameter("size_x", size_x_); + declare_parameter("size_y", size_y_); + declare_parameter("patch_size", patch_size_); + declare_parameter("patch_distance", patch_distance_); + + get_parameter("camera_info_topic", camera_info_topic_); + get_parameter("camera_topic", camera_topic_); + get_parameter("lidar_topic", lidar_topic_); + get_parameter("path_topic", path_topic_); + get_parameter("map_frame", map_frame_id_); + get_parameter("robot_frame", robot_frame_id_); + get_parameter("camera_frame", camera_frame_id_); + get_parameter("size_x", size_x_); + get_parameter("size_y", size_y_); + get_parameter("patch_size", patch_size_); + get_parameter("patch_distance", patch_distance_); } void @@ -181,7 +192,7 @@ get_point_color( } void -GridmapUpdaterNode::update_gridmap( +GridmapUpdaterNode::update_gridmap_elevation( const pcl::PointCloud & pc_map, const pcl::PointCloud & pc_robot, const pcl::PointCloud & pc_camera) @@ -238,17 +249,17 @@ GridmapUpdaterNode::update_gridmap( gridmap_->at("elevation", idx) = em_(idx(0), idx(1)); - 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, *camera_model_, image_rgb_raw_); - if (color > 0) { - cm_(idx(0), idx(1)) = color; - gridmap_->at("RGB", idx) = color; - } - } else { - gridmap_->at("RGB", idx) = cm_(idx(0), idx(1)); - } + // 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, *camera_model_, image_rgb_raw_); + // if (color > 0) { + // cm_(idx(0), idx(1)) = color; + // gridmap_->at("RGB", idx) = color; + // } + // } else { + // // gridmap_->at("RGB", idx) = cm_(idx(0), idx(1)); + // } } } @@ -273,10 +284,10 @@ transform_cloud( std::string error; if (tf_buffer.canTransform( - map_frame, header.frame_id, tf2_ros::fromMsg(header.stamp), 1s, &error)) + map_frame, header.frame_id, tf2::TimePointZero, 1s, &error)) { auto sensor2wf_msg = tf_buffer.lookupTransform( - map_frame, header.frame_id, tf2_ros::fromMsg(header.stamp)); + map_frame, header.frame_id, tf2::TimePointZero); tf2::fromMsg(sensor2wf_msg, sensor2wf); @@ -285,6 +296,7 @@ transform_cloud( return {transformed_cloud, ""}; } else { + std::cerr << "Error transforming point to " << map_frame << "[" << error << "]" << std::endl; return {nullptr, error}; } } @@ -292,6 +304,8 @@ transform_cloud( void GridmapUpdaterNode::pc_callback(sensor_msgs::msg::PointCloud2::UniquePtr pc_in) { + auto start = now(); + pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc_in, *pcl_cloud); @@ -303,6 +317,8 @@ GridmapUpdaterNode::pc_callback(sensor_msgs::msg::PointCloud2::UniquePtr pc_in) *pcl_cloud, pc_in->header, camera_frame_id_, tf_buffer_); + auto ts_0 = now(); + if (cloud_map == nullptr) { RCLCPP_ERROR(get_logger(), "Error transforming pointcloud %s", error_map.c_str()); return; @@ -316,10 +332,18 @@ GridmapUpdaterNode::pc_callback(sensor_msgs::msg::PointCloud2::UniquePtr pc_in) return; } + auto ts_1 = now(); + // reset_gridmap(); - update_gridmap(*cloud_map, *cloud_robot, *cloud_camera); + update_gridmap_elevation(*cloud_map, *cloud_robot, *cloud_camera); + + auto ts_2 = now(); publish_gridmap(pc_in->header.stamp); + + RCLCPP_DEBUG( + get_logger(), "times: %f %f %f ", + (now() - ts_0).seconds(), (now() - ts_1).seconds(), (now() - ts_2).seconds()); } void @@ -353,6 +377,118 @@ GridmapUpdaterNode::topic_callback_info(sensor_msgs::msg::CameraInfo::UniquePtr subscription_info_ = nullptr; } + +std::tuple, bool, std::string> +GridmapUpdaterNode::get_frame_tf( + const std::string & frame, const builtin_interfaces::msg::Time & stamp) +{ + tf2::Stamped map2frame; + std::string error; + + if (tf_buffer_.canTransform( + map_frame_id_, frame, tf2::TimePointZero, 1s, &error)) + { + auto map2frame_msgs = tf_buffer_.lookupTransform( + map_frame_id_, frame, tf2::TimePointZero); + + tf2::fromMsg(map2frame_msgs, map2frame); + + return {map2frame, true, ""}; + } else { + return {map2frame, false, error}; + } +} + +float +get_color_pixel(const cv::Mat & image, const float & x, const float & y) +{ + if (image.type() == CV_8UC3) { + cv::Vec3b color = image.at(static_cast(x), static_cast(y)); + Eigen::Vector3i color_eigen(color[0], color[1], color[2]); + float color_value; + grid_map::colorVectorToValue(color_eigen, color_value); + return color_value; + } else if (image.type() == CV_8UC1) { + float color_value = image.at(static_cast(x), static_cast(y)); + return color_value; + } else if (image.type() == CV_8UC4) { + cv::Vec4b color = image.at(static_cast(y), static_cast(x)); + Eigen::Vector3i color_eigen(color[2], color[1], color[0]); + float color_value; + grid_map::colorVectorToValue(color_eigen, color_value); + return color_value; + } +} + +std::tuple +GridmapUpdaterNode::get_color_in_gridmap( + const cv::Mat & image, + const image_geometry::PinholeCameraModel & camera_model, + const tf2::Stamped & map2camera, + const tf2::Vector3 & gpoint) +{ + tf2::Vector3 cam_point = map2camera.inverse() * gpoint; + + if (std::isnan(cam_point.x()) || cam_point.z() < 0) { + return {color_obstacle_, false}; + } + + cv::Point2d pixel_coords = camera_model.project3dToPixel( + cv::Point3d(cam_point.x(), cam_point.y(), cam_point.z())); + + const float & x = pixel_coords.x; + const float & y = pixel_coords.y; + const int & width = image.cols; + const int & height = image.rows; + + if (x < 0 || x >= width - 1 || y < 0 || y >= height - 1) { + return {color_obstacle_, false}; + } + + return {get_color_pixel(image, x, y), true}; +} + + +void +GridmapUpdaterNode::update_gridmap_texture( + const cv::Mat & image, + const image_geometry::PinholeCameraModel & camera_model, + builtin_interfaces::msg::Time stamp) +{ + auto [map2robot, success_r, error_r] = get_frame_tf(robot_frame_id_, stamp); + auto [map2camera, success_c, error_c] = get_frame_tf(camera_frame_id_, stamp); + + if (!success_r) { + RCLCPP_ERROR(get_logger(), "update_gridmap_texture robot error: [%s]", error_r.c_str()); + return; + } + if (!success_c) { + RCLCPP_ERROR(get_logger(), "update_gridmap_texture robot camera: [%s]", error_c.c_str()); + return; + } + + auto position = map2robot.getOrigin(); + + grid_map::Position center(position.x(), position.y()); + double radius = 10.0; + + for (grid_map::CircleIterator it(*gridmap_, center, radius); !it.isPastEnd(); ++it) { + grid_map::Position gpos; + gridmap_->getPosition(*it, gpos); + grid_map::Index idx; + gridmap_->getIndex(gpos, idx); + + tf2::Vector3 gpoint(gpos.x(), gpos.y(), em_(idx(0), idx(1))); + + auto [color, in_image] = get_color_in_gridmap(image, camera_model, map2camera, gpoint); + + if (in_image) { + cm_(idx(0), idx(1)) = color; + gridmap_->at("RGB", idx) = cm_(idx(0), idx(1)); + } + } +} + void GridmapUpdaterNode::image_callback(sensor_msgs::msg::Image::UniquePtr msg) { @@ -374,7 +510,67 @@ GridmapUpdaterNode::image_callback(sensor_msgs::msg::Image::UniquePtr msg) return; } - image_rgb_raw_ = image_rgb_ptr->image; + update_gridmap_texture(image_rgb_ptr->image, *camera_model_, msg->header.stamp); + publish_gridmap(msg->header.stamp); +} + +std::tuple +GridmapUpdaterNode::create_patch_in_pos(const tf2::Stamped & pos) +{ + bool success; + grid_map::Position center_pos(pos.getOrigin().x(), pos.getOrigin().y()); + int size = patch_size_ / resolution_gridmap_; + + grid_map::GridMap ret = gridmap_->getSubmap(center_pos, grid_map::Length(size, size), success); + ret.setFrameId(robot_frame_id_); + + return {ret, success}; +} + +void +GridmapUpdaterNode::publish_patch(const grid_map::GridMap & patch) +{ + std::unique_ptr msg; + msg = grid_map::GridMapRosConverter::toMessage(patch); + msg->header.frame_id = robot_frame_id_; + msg->header.stamp = now(); + + patch_pub_->publish(std::move(msg)); +} + +void +GridmapUpdaterNode::control_cycle() +{ + tf2::Stamped current_map2robot; + auto [map2robot0, success_r0, error_r0] = get_frame_tf(robot_frame_id_, now()); + + if (!started_odom_) { + if (success_r0) { + last_map2robot_ = map2robot0; + started_odom_ = true; + } else { + return; + } + } + + auto [map2robot, success_r, error_r] = get_frame_tf(robot_frame_id_, now()); + + if (!success_r) { + RCLCPP_ERROR(get_logger(), "update_gridmap_texture robot error: [%s]", error_r.c_str()); + return; + } + + auto trans = last_map2robot_.inverse() * map2robot; + + if (trans.getOrigin().length() > patch_distance_) { + auto [patch, success] = create_patch_in_pos(map2robot); + if (success) { + std::cerr << "Publishing" << std::endl; + publish_patch(patch); + last_map2robot_ = map2robot0; + } + } } + } // namespace local_navigation diff --git a/local_navigation/src/tools/summit_correct_zed_tf.cpp b/local_navigation/src/tools/summit_correct_zed_tf.cpp new file mode 100644 index 0000000..de0b38c --- /dev/null +++ b/local_navigation/src/tools/summit_correct_zed_tf.cpp @@ -0,0 +1,107 @@ +// 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. + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/static_transform_broadcaster.h" + +#include "tf2_msgs/msg/tf_message.hpp" + +#include "rclcpp/rclcpp.hpp" + + +class ZedTFFixer : public rclcpp::Node +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ZedTFFixer) + + explicit ZedTFFixer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("zed_tf_fixer", options) + { + tf_static_pub_ = create_publisher( + "/tf_static", rclcpp::QoS(100).reliable().transient_local()); + tf_pub_ = create_publisher( + "/tf", rclcpp::QoS(100).reliable()); + + tf_static_sub_ = create_subscription( + "/rosbag/tf_static", rclcpp::QoS(100).reliable().transient_local(), + [&] (tf2_msgs::msg::TFMessage::UniquePtr msg) { + // std::cerr << "-" << std::endl; + tf2_msgs::msg::TFMessage fixed_msg; + + for (const auto & tf : msg->transforms) { + if (tf.header.frame_id != "base_link" && tf.child_frame_id != "base_link") { + fixed_msg.transforms.push_back(tf); + } else { + RCLCPP_INFO_STREAM( + get_logger(), + "Filtering a tf: " << tf.header.frame_id << " -> " << tf.child_frame_id); + } + } + + geometry_msgs::msg::TransformStamped fix_tf; + fix_tf.header.stamp = msg->transforms[0].header.stamp; + fix_tf.header.frame_id = "robot_front_camera_base_link"; + fix_tf.header.stamp = now(); + fix_tf.child_frame_id = "zed2_camera_center"; + + fixed_msg.transforms.push_back(fix_tf); + tf_static_pub_->publish(fixed_msg); + }); + + tf_sub_ = create_subscription( + "/rosbag/tf", rclcpp::QoS(100).reliable(), + [&] (tf2_msgs::msg::TFMessage::UniquePtr msg) { + // std::cerr << "*" << std::endl; + tf2_msgs::msg::TFMessage fixed_msg; + + for (const auto & tf : msg->transforms) { + if (tf.header.frame_id != "base_link" && tf.child_frame_id != "base_link") { + fixed_msg.transforms.push_back(tf); + } else { + RCLCPP_INFO_STREAM( + get_logger(), + "Filtering a tf: " << tf.header.frame_id << " -> " << tf.child_frame_id); + } + } + + geometry_msgs::msg::TransformStamped fix_tf; + fix_tf.header.stamp = msg->transforms[0].header.stamp; + fix_tf.header.frame_id = "robot_front_camera_base_link"; + fix_tf.header.stamp = now(); + fix_tf.child_frame_id = "zed2_camera_center"; + + fixed_msg.transforms.push_back(fix_tf); + tf_pub_->publish(fixed_msg); + }); + } + +private: + rclcpp::Subscription::SharedPtr tf_sub_; + rclcpp::Publisher::SharedPtr tf_pub_; + rclcpp::Subscription::SharedPtr tf_static_sub_; + rclcpp::Publisher::SharedPtr tf_static_pub_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto zed_tf_fixer_node = ZedTFFixer::make_shared(); + + rclcpp::spin(zed_tf_fixer_node); + + rclcpp::shutdown(); + return 0; +} From bd1144dc46ff70d9a3b302b613d17d2eb18cad5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 12 Aug 2024 14:16:36 +0200 Subject: [PATCH 2/2] Fix segfault when camara model is not still available MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- local_navigation/config/params_go2.yaml | 2 ++ local_navigation/launch/local_navigation_go2.launch.py | 2 +- local_navigation/src/local_navigation/GridmapUpdaterNode.cpp | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/local_navigation/config/params_go2.yaml b/local_navigation/config/params_go2.yaml index 293fa4e..1a8d7b4 100644 --- a/local_navigation/config/params_go2.yaml +++ b/local_navigation/config/params_go2.yaml @@ -7,6 +7,8 @@ gridmap_updater_node: map_frame: map robot_frame: base_link camera_frame: camera_color_optical_frame + size_x: 200.0 + size_y: 200.0 scan_matcher: ros__parameters: diff --git a/local_navigation/launch/local_navigation_go2.launch.py b/local_navigation/launch/local_navigation_go2.launch.py index 066e077..44dfcb8 100755 --- a/local_navigation/launch/local_navigation_go2.launch.py +++ b/local_navigation/launch/local_navigation_go2.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): executable='local_navigation_program', output='screen', parameters=[param_file], - # prefix=['xterm -e gdb -ex run --args'], + # prefix=['xterm -e gdb -ex run --args'], # prefix=['perf record --call-graph dwarf -o perf.data'], arguments=[], remappings=[]) diff --git a/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp b/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp index a681216..c94fb91 100644 --- a/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp +++ b/local_navigation/src/local_navigation/GridmapUpdaterNode.cpp @@ -492,6 +492,8 @@ GridmapUpdaterNode::update_gridmap_texture( void GridmapUpdaterNode::image_callback(sensor_msgs::msg::Image::UniquePtr msg) { + if (camera_model_ == nullptr) {return;} + cv_bridge::CvImagePtr image_rgb_ptr; try {