wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update some debug visualization; add custom msg

Browse Source
This commit is contained in:
黄JY
2026-04-10 16:56:08 +08:00
parent 62d2ce8cc6
commit a2c655bb12
8 changed files with 187 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+53
View File
@@ -215,6 +215,15 @@ if(ODIN_TARGET_OBSERVATION_READY)
message(STATUS "Target observation support enabled in odin_ros_driver")
endif()
function(set_runtime_search_path target_name runtime_path)
if(TARGET ${target_name})
set_target_properties(${target_name} PROPERTIES
BUILD_RPATH "${runtime_path}"
INSTALL_RPATH "${runtime_path}"
)
endif()
endfunction()
# ===== ROS1 Configuration =====
if(ROS_VERSION STREQUAL "ROS1")
message(STATUS "Configuring for ROS1 build")
@@ -323,9 +332,11 @@ if(ROS_VERSION STREQUAL "ROS1")
# ===== ROS2 Configuration =====
elseif(ROS_VERSION STREQUAL "ROS2")
message(STATUS "Configuring for ROS2 build")
set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries for ROS2 targets" FORCE)
# Find all necessary ROS2 packages
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
@@ -339,6 +350,16 @@ elseif(ROS_VERSION STREQUAL "ROS2")
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/TargetObservation.msg"
DEPENDENCIES std_msgs geometry_msgs
)
rosidl_get_typesupport_target(odin_ros_driver_interfaces_target
${PROJECT_NAME} "rosidl_typesupport_cpp")
rosidl_get_typesupport_target(odin_ros_driver_fastrtps_cpp_target
${PROJECT_NAME} "rosidl_typesupport_fastrtps_cpp")
# Create executable
add_executable(host_sdk_sample
@@ -423,8 +444,11 @@ elseif(ROS_VERSION STREQUAL "ROS2")
src/cloud_reprojection_processing.cpp
)
target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2)
target_link_options(cloud_reprojection_ros2_node PRIVATE "-Wl,--no-as-needed")
target_link_libraries(cloud_reprojection_ros2_node
cloud_reprojector_ros2
${odin_ros_driver_interfaces_target}
${odin_ros_driver_fastrtps_cpp_target}
${OpenCV_LIBS}
${PCL_LIBRARIES}
yaml-cpp
@@ -457,9 +481,31 @@ elseif(ROS_VERSION STREQUAL "ROS2")
message_filters
)
foreach(ros2_runtime_target
host_sdk_sample
pcd2depth_ros2_node
cloud_reprojection_ros2_node
image_overlay_node
)
set_runtime_search_path(${ros2_runtime_target} "\$ORIGIN/..")
endforeach()
foreach(ros2_library_target
pointcloud_depth_converter_ros2
depth_image_ros2_node_lib
cloud_reprojector_ros2
)
set_runtime_search_path(${ros2_library_target} "\$ORIGIN")
endforeach()
set_runtime_search_path(motcpp "\$ORIGIN")
# Installation rules - ensure all install targets are defined before ament_package()
# Install executable
install(TARGETS
pointcloud_depth_converter_ros2
depth_image_ros2_node_lib
cloud_reprojector_ros2
host_sdk_sample
pcd2depth_ros2_node
cloud_reprojection_ros2_node
@@ -469,6 +515,12 @@ elseif(ROS_VERSION STREQUAL "ROS2")
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
if(TARGET motcpp)
install(TARGETS motcpp
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
)
endif()
# Install package.xml
install(FILES package.xml
@@ -505,6 +557,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
# Declare dependencies
ament_export_dependencies(
rclcpp
rosidl_default_runtime
std_msgs
sensor_msgs
nav_msgs
config/odin_ros2.rviz
+78 -16
View File
@@ -11,8 +11,9 @@ Panels:
- /Odometry1
- /slam1
- /dense_depth_demo1
- /Prediction1
Splitter Ratio: 0.5
Tree Height: 593
Tree Height: 865
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -48,7 +49,7 @@ Visualization Manager:
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/Image
Enabled: true
Enabled: false
Max Value: 1
Median window: 5
Min Value: 0
@@ -60,7 +61,7 @@ Visualization Manager:
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/image
Value: true
Value: false
- Class: rviz_default_plugins/Image
Enabled: false
Max Value: 1
@@ -407,6 +408,67 @@ Visualization Manager:
{}
Update Interval: 0
Value: true
- Class: rviz_common/Group
Displays:
- Class: rviz_default_plugins/Marker
Enabled: true
Name: Marker
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /target/pred_trajectory
Value: true
- Class: rviz_default_plugins/Image
Enabled: false
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /target/pred_image
Value: false
- Alpha: 1
Class: rviz_default_plugins/PointStamped
Color: 224; 27; 36
Enabled: true
History Length: 10
Name: target_pos_world
Radius: 0.30000001192092896
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/sync/target_pos_world
Value: true
- Alpha: 1
Class: rviz_default_plugins/PointStamped
Color: 204; 41; 204
Enabled: true
History Length: 1
Name: target_pos_cam
Radius: 0.20000000298023224
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/sync/target_pos_cam
Value: true
Enabled: true
Name: Prediction
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -452,26 +514,26 @@ Visualization Manager:
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 10.9336576461792
Class: rviz_default_plugins/ThirdPersonFollower
Distance: 23.027545928955078
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.2568470537662506
Y: 2.1451337337493896
Z: 0.3774382472038269
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.3953983187675476
Target Frame: odom
Value: Orbit (rviz)
Yaw: 3.230407953262329
Pitch: 0.5153985023498535
Target Frame: odin1_base_link
Value: ThirdPersonFollower (rviz_default_plugins)
Yaw: 2.1653902530670166
Saved: ~
Window Geometry:
Displays:
@@ -483,16 +545,16 @@ Window Geometry:
collapsed: false
Image_undistort:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001f7000001700000002800fffffffb0000000a0049006d006100670065000000036d0000006e0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000044b0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 540
Y: 124
Width: 1850
X: 70
Y: 27
cloudslam_reprojected:
collapsed: false
dense_depth_image:
include/cloud_reprojection_ros_node.hpp
+2 -3
View File
@@ -25,6 +25,7 @@ limitations under the License.
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "odin_ros_driver/msg/target_observation.hpp"
#else
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
@@ -87,7 +88,6 @@ private:
std::string sync_wiwc_topic_;
std::string sync_image_topic_;
std::string sync_overlay_image_topic_;
std::string sync_target_track_id_topic_;
std::string sync_target_observation_topic_;
std::string sync_target_pos_cam_topic_;
std::string sync_target_pos_world_topic_;
@@ -100,8 +100,7 @@ private:
rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_; // optional if send_overlay_
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_; // optional if publish_combined_compressed_
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr target_track_id_pub_;
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr target_observation_pub_;
rclcpp::Publisher<odin_ros_driver::msg::TargetObservation>::SharedPtr target_observation_pub_;
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_cam_pub_;
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_world_pub_;
include/yaml_parser.h
+13 -2
View File
@@ -87,9 +87,20 @@ private:
std::map<std::string, std::string> register_keys_str_val_;
std::map<std::string, ParameterValue> custom_parameters_;
std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name", "image_mask_abs_path"};
std::unordered_set<std::string> allowed_key_w_str_val = {
"relocalization_map_abs_path",
"mapping_result_dest_dir",
"mapping_result_file_name",
"image_mask_abs_path",
"overlay_reprojected_topic",
"overlay_camera_topic",
"overlay_output_topic",
"sync_camera_topic",
"sync_topic_prefix",
"combined_compressed_topic"
};
};
}
#endif
#endif
msg/TargetObservation.msg
+11
View File
@@ -0,0 +1,11 @@
std_msgs/Header header
bool valid
int32 track_id
int32 detection_index
float32 confidence
float32 depth
float32 depth_confidence
float32[4] bbox_xyxy
float32[51] keypoints_xyc
geometry_msgs/Point target_pos_cam
geometry_msgs/Point target_pos_world
package.xml
+4 -1
View File
@@ -7,6 +7,7 @@
<license>Apache 2.0</license>
<!-- ROS2 uses colcon as the build tool -->
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<!-- ROS2 dependencies -->
<depend>rclcpp</depend>
<!-- System dependencies -->
@@ -21,8 +22,10 @@
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<!-- Specify build type as ament -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
</package>
package_ros2.xml
+4 -1
View File
@@ -7,6 +7,7 @@
<license>Apache 2.0</license>
<!-- ROS2 uses colcon as the build tool -->
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<!-- ROS2 dependencies -->
<depend>rclcpp</depend>
<!-- System dependencies -->
@@ -21,8 +22,10 @@
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<!-- Specify build type as ament -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
</package>
src/cloud_reprojection_ros.cpp
+22 -31
View File
@@ -31,6 +31,7 @@ limitations under the License.
#include <geometry_msgs/msg/point_stamped.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#include "odin_ros_driver/msg/target_observation.hpp"
#endif
namespace {
@@ -131,7 +132,6 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
<< "\n process_target_observation: " << (enable_target_observation_ ? "on" : "off")
<< "\n debug: " << (debug_target_observation_ ? "on" : "off")
<< "\n target_track_id_topic: " << sync_target_track_id_topic_
<< "\n target_observation_topic: " << sync_target_observation_topic_
<< "\n target_pos_cam_topic: " << sync_target_pos_cam_topic_
<< "\n target_pos_world_topic: " << sync_target_pos_world_topic_
@@ -163,9 +163,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
}
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
if (enable_target_observation_) {
target_track_id_pub_ = this->create_publisher<std_msgs::msg::Int32>(
sync_target_track_id_topic_, 10);
target_observation_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(
target_observation_pub_ = this->create_publisher<odin_ros_driver::msg::TargetObservation>(
sync_target_observation_topic_, 10);
target_pos_cam_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
sync_target_pos_cam_topic_, 10);
@@ -173,8 +171,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
sync_target_pos_world_topic_, 10);
RCLCPP_INFO(
this->get_logger(),
"Target observation publishers created successfully | track_id=%s | observation=%s | pos_cam=%s | pos_world=%s",
sync_target_track_id_topic_.c_str(),
"Target observation publishers created successfully | observation=%s | pos_cam=%s | pos_world=%s",
sync_target_observation_topic_.c_str(),
sync_target_pos_cam_topic_.c_str(),
sync_target_pos_world_topic_.c_str());
@@ -236,7 +233,6 @@ void CloudReprojectionRosNode::loadParameters()
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
sync_target_track_id_topic_ = sync_topic_prefix_ + "/target_track_id";
sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
sync_target_pos_cam_topic_ = sync_topic_prefix_ + "/target_pos_cam";
sync_target_pos_world_topic_ = sync_topic_prefix_ + "/target_pos_world";
@@ -442,7 +438,8 @@ void CloudReprojectionRosNode::syncCallback(
cloud_cam_msg.header = image_msg->header;
cloud_cam_msg.header.stamp = sync_stamp;
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
// cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
cloud_cam_msg.header.frame_id = odom_msg->child_frame_id; // odin1_base_link
}
@@ -567,29 +564,23 @@ void CloudReprojectionRosNode::syncCallback(
}
if (target_observation.valid) {
std_msgs::msg::Int32 track_id_msg;
track_id_msg.data = target_observation.track_id;
target_track_id_pub_->publish(track_id_msg);
std_msgs::msg::Float32MultiArray observation_msg;
observation_msg.data.reserve(4 + 17 * 3 + 3 + 3 + 3);
observation_msg.data.insert(
observation_msg.data.end(),
target_observation.bbox_xyxy.begin(),
target_observation.bbox_xyxy.end());
observation_msg.data.insert(
observation_msg.data.end(),
target_observation.keypoints_xyc.begin(),
target_observation.keypoints_xyc.end());
observation_msg.data.push_back(target_observation.target_pos_cam.x());
observation_msg.data.push_back(target_observation.target_pos_cam.y());
observation_msg.data.push_back(target_observation.target_pos_cam.z());
observation_msg.data.push_back(target_observation.target_pos_world.x());
observation_msg.data.push_back(target_observation.target_pos_world.y());
observation_msg.data.push_back(target_observation.target_pos_world.z());
observation_msg.data.push_back(target_observation.confidence);
observation_msg.data.push_back(target_observation.depth);
observation_msg.data.push_back(target_observation.depth_confidence);
odin_ros_driver::msg::TargetObservation observation_msg;
observation_msg.header = image_msg->header;
observation_msg.header.stamp = sync_stamp;
observation_msg.valid = target_observation.valid;
observation_msg.track_id = target_observation.track_id;
observation_msg.detection_index = target_observation.detection_index;
observation_msg.confidence = target_observation.confidence;
observation_msg.depth = target_observation.depth;
observation_msg.depth_confidence = target_observation.depth_confidence;
observation_msg.bbox_xyxy = target_observation.bbox_xyxy;
observation_msg.keypoints_xyc = target_observation.keypoints_xyc;
observation_msg.target_pos_cam.x = target_observation.target_pos_cam.x();
observation_msg.target_pos_cam.y = target_observation.target_pos_cam.y();
observation_msg.target_pos_cam.z = target_observation.target_pos_cam.z();
observation_msg.target_pos_world.x = target_observation.target_pos_world.x();
observation_msg.target_pos_world.y = target_observation.target_pos_world.y();
observation_msg.target_pos_world.z = target_observation.target_pos_world.z();
target_observation_pub_->publish(observation_msg);
geometry_msgs::msg::PointStamped pos_cam_msg;