Skip to content
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

refactor(image_projection_based_fusion): convert input topic names to ROS parameters #4247

Merged
merged 7 commits into from
Jul 20, 2023
Merged

refactor(image_projection_based_fusion): convert input topic names to ROS parameters #4247

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
e32dd46
init commit
ismetatabay Jul 11, 2023
5e6ca82
style(pre-commit): autofix
pre-commit-ci[bot] Jul 11, 2023
e2c2e88
update pointpainting and roi_detected_object_fusion_launch
ismetatabay Jul 12, 2023
150e127
Merge branch 'main' into refactor/image-projection-based-fusion
ismetatabay Jul 17, 2023
379bb25
Merge branch 'main' into refactor/image-projection-based-fusion
ismetatabay Jul 20, 2023
ed4f4f2
correct argument and param definitions
ismetatabay Jul 20, 2023
5a0cc3a
Merge branch 'main' into refactor/image-projection-based-fusion
ismetatabay Jul 20, 2023
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 perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include <boost/circular_buffer.hpp>

#include <memory>
#include <string>
#include <vector>

namespace image_projection_based_fusion
Expand All @@ -39,7 +40,8 @@ class Debugger
{
public:
explicit Debugger(
rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size);
rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size,
std::vector<std::string> input_camera_topics);

void publishImage(const std::size_t image_id, const rclcpp::Time & stamp);

Expand All @@ -55,6 +57,7 @@ class Debugger

rclcpp::Node * node_ptr_;
std::shared_ptr<image_transport::ImageTransport> image_transport_;
std::vector<std::string> input_camera_topics_;
std::vector<image_transport::Subscriber> image_subs_;
std::vector<image_transport::Publisher> image_pubs_;
std::vector<boost::circular_buffer<sensor_msgs::msg::Image::ConstSharedPtr>> image_buffers_;
Expand Down
6 changes: 4 additions & 2 deletions ...ption/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,13 +100,15 @@ class FusionNode : public rclcpp::Node
rclcpp::TimerBase::SharedPtr timer_;
double timeout_ms_{};
double match_threshold_ms_{};
std::vector<std::string> input_rois_topics_;
std::vector<std::string> input_camera_info_topics_;
std::vector<std::string> input_camera_topics_;

/** \brief A vector of subscriber. */
typename rclcpp::Subscription<Msg>::SharedPtr sub_;
std::vector<rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr> rois_subs_;

/** \brief Input point cloud topics. */
std::vector<std::string> input_topics_;
// offsets between cameras and the lidars
std::vector<double> input_offset_ms_;

// cache for fusion
Expand Down
52 changes: 26 additions & 26 deletions perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,32 +38,6 @@
<arg name="input/image6" default="/image_raw6"/>
<arg name="input/image7" default="/image_raw7"/>
<group>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/rois0" to="$(var input/rois0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/camera_info0" to="$(var input/camera_info0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/rois2" to="$(var input/rois2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/camera_info2" to="$(var input/camera_info2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/rois3" to="$(var input/rois3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/camera_info3" to="$(var input/camera_info3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/rois4" to="$(var input/rois4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/camera_info4" to="$(var input/camera_info4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/rois5" to="$(var input/rois5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/camera_info5" to="$(var input/camera_info5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/rois6" to="$(var input/rois6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/camera_info6" to="$(var input/camera_info6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/rois7" to="$(var input/rois7)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/camera_info7" to="$(var input/camera_info7)"/>

<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '1' &quot;)" from="input/image_raw0" to="$(var input/image0)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '2' &quot;)" from="input/image_raw1" to="$(var input/image1)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '3' &quot;)" from="input/image_raw2" to="$(var input/image2)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '4' &quot;)" from="input/image_raw3" to="$(var input/image3)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '5' &quot;)" from="input/image_raw4" to="$(var input/image4)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '6' &quot;)" from="input/image_raw5" to="$(var input/image5)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '7' &quot;)" from="input/image_raw6" to="$(var input/image6)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '8' &quot;)" from="input/image_raw7" to="$(var input/image7)"/>

<node pkg="image_projection_based_fusion" exec="pointpainting_fusion_node" name="pointpainting" output="screen">
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
<remap from="~/output/objects" to="$(var output/objects)"/>
Expand All @@ -80,6 +54,32 @@
<param from="$(var class_remapper_param_path)"/>
<param name="rois_number" value="$(var input/rois_number)"/>

<!-- rois, camera and info -->
<param name="input/rois0" value="$(var input/rois0)"/>
<param name="input/camera_info0" value="$(var input/camera_info0)"/>
<param name="input/image0" value="$(var input/image0)"/>
<param name="input/rois1" value="$(var input/rois1)"/>
<param name="input/camera_info1" value="$(var input/camera_info1)"/>
<param name="input/image1" value="$(var input/image1)"/>
<param name="input/rois2" value="$(var input/rois2)"/>
<param name="input/camera_info2" value="$(var input/camera_info2)"/>
<param name="input/image2" value="$(var input/image2)"/>
<param name="input/rois3" value="$(var input/rois3)"/>
<param name="input/camera_info3" value="$(var input/camera_info3)"/>
<param name="input/image3" value="$(var input/image3)"/>
<param name="input/rois4" value="$(var input/rois4)"/>
<param name="input/camera_info4" value="$(var input/camera_info4)"/>
<param name="input/image4" value="$(var input/image4)"/>
<param name="input/rois5" value="$(var input/rois5)"/>
<param name="input/camera_info5" value="$(var input/camera_info5)"/>
<param name="input/image5" value="$(var input/image5)"/>
<param name="input/rois6" value="$(var input/rois6)"/>
<param name="input/camera_info6" value="$(var input/camera_info6)"/>
<param name="input/image6" value="$(var input/image6)"/>
<param name="input/rois7" value="$(var input/rois7)"/>
<param name="input/camera_info7" value="$(var input/camera_info7)"/>
<param name="input/image7" value="$(var input/image7)"/>

<!-- debug -->
<param name="debug_mode" value="$(var debug_mode)"/>
</node>
Expand Down
52 changes: 26 additions & 26 deletions perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,32 +44,6 @@
<arg name="input/image6" default="/image_raw6"/>
<arg name="input/image7" default="/image_raw7"/>
<group>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/rois0" to="$(var input/rois0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/camera_info0" to="$(var input/camera_info0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/rois2" to="$(var input/rois2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/camera_info2" to="$(var input/camera_info2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/rois3" to="$(var input/rois3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/camera_info3" to="$(var input/camera_info3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/rois4" to="$(var input/rois4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/camera_info4" to="$(var input/camera_info4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/rois5" to="$(var input/rois5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/camera_info5" to="$(var input/camera_info5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/rois6" to="$(var input/rois6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/camera_info6" to="$(var input/camera_info6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/rois7" to="$(var input/rois7)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/camera_info7" to="$(var input/camera_info7)"/>

<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '1' &quot;)" from="input/image_raw0" to="$(var input/image0)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '2' &quot;)" from="input/image_raw1" to="$(var input/image1)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '3' &quot;)" from="input/image_raw2" to="$(var input/image2)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '4' &quot;)" from="input/image_raw3" to="$(var input/image3)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '5' &quot;)" from="input/image_raw4" to="$(var input/image4)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '6' &quot;)" from="input/image_raw5" to="$(var input/image5)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '7' &quot;)" from="input/image_raw6" to="$(var input/image6)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '8' &quot;)" from="input/image_raw7" to="$(var input/image7)"/>

<node pkg="image_projection_based_fusion" exec="roi_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
<param name="use_iou" value="true"/>
<param name="use_iou_x" value="false"/>
Expand All @@ -84,6 +58,32 @@
<remap from="input" to="$(var input/clusters)"/>
<remap from="output" to="$(var output/clusters)"/>

<!-- rois, camera and info -->
<param name="input/rois0" value="$(var input/rois0)"/>
<param name="input/camera_info0" value="$(var input/camera_info0)"/>
<param name="input/image0" value="$(var input/image0)"/>
<param name="input/rois1" value="$(var input/rois1)"/>
<param name="input/camera_info1" value="$(var input/camera_info1)"/>
<param name="input/image1" value="$(var input/image1)"/>
<param name="input/rois2" value="$(var input/rois2)"/>
<param name="input/camera_info2" value="$(var input/camera_info2)"/>
<param name="input/image2" value="$(var input/image2)"/>
<param name="input/rois3" value="$(var input/rois3)"/>
<param name="input/camera_info3" value="$(var input/camera_info3)"/>
<param name="input/image3" value="$(var input/image3)"/>
<param name="input/rois4" value="$(var input/rois4)"/>
<param name="input/camera_info4" value="$(var input/camera_info4)"/>
<param name="input/image4" value="$(var input/image4)"/>
<param name="input/rois5" value="$(var input/rois5)"/>
<param name="input/camera_info5" value="$(var input/camera_info5)"/>
<param name="input/image5" value="$(var input/image5)"/>
<param name="input/rois6" value="$(var input/rois6)"/>
<param name="input/camera_info6" value="$(var input/camera_info6)"/>
<param name="input/image6" value="$(var input/image6)"/>
<param name="input/rois7" value="$(var input/rois7)"/>
<param name="input/camera_info7" value="$(var input/camera_info7)"/>
<param name="input/image7" value="$(var input/image7)"/>

<!-- debug -->
<param name="debug_mode" value="$(var debug_mode)"/>
<param name="filter_scope_minx" value="$(var filter_scope_minx)"/>
Expand Down
54 changes: 28 additions & 26 deletions perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,33 +34,8 @@
<arg name="input/image5" default="/image_raw5"/>
<arg name="input/image6" default="/image_raw6"/>
<arg name="input/image7" default="/image_raw7"/>
<group>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/rois0" to="$(var input/rois0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '1' &quot;)" from="input/camera_info0" to="$(var input/camera_info0)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/rois2" to="$(var input/rois2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '3' &quot;)" from="input/camera_info2" to="$(var input/camera_info2)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/rois3" to="$(var input/rois3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '4' &quot;)" from="input/camera_info3" to="$(var input/camera_info3)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/rois4" to="$(var input/rois4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '5' &quot;)" from="input/camera_info4" to="$(var input/camera_info4)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/rois5" to="$(var input/rois5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '6' &quot;)" from="input/camera_info5" to="$(var input/camera_info5)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/rois6" to="$(var input/rois6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '7' &quot;)" from="input/camera_info6" to="$(var input/camera_info6)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/rois7" to="$(var input/rois7)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '8' &quot;)" from="input/camera_info7" to="$(var input/camera_info7)"/>

<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '1' &quot;)" from="input/image_raw0" to="$(var input/image0)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '2' &quot;)" from="input/image_raw1" to="$(var input/image1)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '3' &quot;)" from="input/image_raw2" to="$(var input/image2)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '4' &quot;)" from="input/image_raw3" to="$(var input/image3)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '5' &quot;)" from="input/image_raw4" to="$(var input/image4)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '6' &quot;)" from="input/image_raw5" to="$(var input/image5)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '7' &quot;)" from="input/image_raw6" to="$(var input/image6)"/>
<set_remap if="$(eval &quot; '$(var input_rois_number)' >= '8' &quot;)" from="input/image_raw7" to="$(var input/image7)"/>

<group>
<node pkg="image_projection_based_fusion" exec="roi_detected_object_fusion_node" name="roi_detected_object_fusion" output="screen">
<param name="rois_number" value="$(var input/rois_number)"/>
<param from="$(var sync_param_path)"/>
Expand All @@ -70,6 +45,33 @@
<param name="iou_threshold" value="0.3"/>
<param name="use_roi_probability" value="false"/>
<param name="roi_probability_threshold" value="0.5"/>
<param name="min_iou_threshold" value="0.5"/>

<!-- rois, camera and info -->
<param name="input/rois0" value="$(var input/rois0)"/>
<param name="input/camera_info0" value="$(var input/camera_info0)"/>
<param name="input/image0" value="$(var input/image0)"/>
<param name="input/rois1" value="$(var input/rois1)"/>
<param name="input/camera_info1" value="$(var input/camera_info1)"/>
<param name="input/image1" value="$(var input/image1)"/>
<param name="input/rois2" value="$(var input/rois2)"/>
<param name="input/camera_info2" value="$(var input/camera_info2)"/>
<param name="input/image2" value="$(var input/image2)"/>
<param name="input/rois3" value="$(var input/rois3)"/>
<param name="input/camera_info3" value="$(var input/camera_info3)"/>
<param name="input/image3" value="$(var input/image3)"/>
<param name="input/rois4" value="$(var input/rois4)"/>
<param name="input/camera_info4" value="$(var input/camera_info4)"/>
<param name="input/image4" value="$(var input/image4)"/>
<param name="input/rois5" value="$(var input/rois5)"/>
<param name="input/camera_info5" value="$(var input/camera_info5)"/>
<param name="input/image5" value="$(var input/image5)"/>
<param name="input/rois6" value="$(var input/rois6)"/>
<param name="input/camera_info6" value="$(var input/camera_info6)"/>
<param name="input/image6" value="$(var input/image6)"/>
<param name="input/rois7" value="$(var input/rois7)"/>
<param name="input/camera_info7" value="$(var input/camera_info7)"/>
<param name="input/image7" value="$(var input/image7)"/>

<!-- debug -->
<param name="debug_mode" value="$(var debug_mode)"/>
Expand Down
7 changes: 4 additions & 3 deletions perception/image_projection_based_fusion/src/debugger.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,14 +39,15 @@ void drawRoiOnImage(
namespace image_projection_based_fusion
{
Debugger::Debugger(
rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size)
: node_ptr_(node_ptr)
rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size,
std::vector<std::string> input_camera_topics)
: node_ptr_(node_ptr), input_camera_topics_{input_camera_topics}
{
image_buffers_.resize(image_num);
image_buffer_size_ = image_buffer_size;
for (std::size_t img_i = 0; img_i < image_num; ++img_i) {
auto sub = image_transport::create_subscription(
node_ptr, "input/image_raw" + std::to_string(img_i),
node_ptr, input_camera_topics.at(img_i),
std::bind(&Debugger::imageCallback, this, std::placeholders::_1, img_i), "raw",
rmw_qos_profile_sensor_data);
image_subs_.push_back(sub);
Expand Down
Loading