Skip to content

Fix camera get patch #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
fmrico wants to merge 2 commits into
base: rolling
Choose a base branch
from
Open

Fix camera get patch #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f438c17
Fix image and get patches
fmrico Aug 12, 2024
bd1144d
Fix segfault when camara model is not still available
fmrico Aug 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion local_navigation/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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}
)

Expand Down
4 changes: 3 additions & 1 deletion local_navigation/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
59 changes: 59 additions & 0 deletions local_navigation/config/params_go2.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
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
size_x: 200.0
size_y: 200.0

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
9 changes: 0 additions & 9 deletions local_navigation/config/params_perro.yaml
View file
Open in desktop

This file was deleted.

50 changes: 50 additions & 0 deletions local_navigation/config/params_summit.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
49 changes: 36 additions & 13 deletions local_navigation/include/local_navigation/GridmapUpdaterNode.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand All @@ -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
Expand Down
1 change: 0 additions & 1 deletion local_navigation/install/.colcon_install_layout
View file
Open in desktop

This file was deleted.

8 changes: 7 additions & 1 deletion local_navigation/launch/local_navigation.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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',
Expand Down
40 changes: 22 additions & 18 deletions ...n/launch/local_navigation_perro.launch.py → ...ion/launch/local_navigation_go2.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,40 +17,44 @@
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',
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=[])

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
31 changes: 22 additions & 9 deletions local_navigation/launch/local_navigation_summit.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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',
Expand All @@ -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
Loading